def __init__(self):
"""Class constructor."""
ThrusterManager.__init__(self)
self.last_update = rospy.Time.now()
# Subscriber to the wrench to be applied on the UUV
self.input_sub = rospy.Subscriber('thruster_manager/input', Wrench,
self.input_callback)
self.thruster_info_service = rospy.Service(
'thruster_manager/get_thrusters_info', ThrusterManagerInfo,
self.get_thruster_info)
self.curve_calc_service = rospy.Service(
'thruster_manager/get_thruster_curve', GetThrusterCurve,
self.get_thruster_curve)
self.set_thruster_manager_config_service = rospy.Service(
'thruster_manager/set_config', SetThrusterManagerConfig,
self.set_config)
self.get_thruster_manager_config_service = rospy.Service(
'thruster_manager/get_config', GetThrusterManagerConfig,
self.get_config)
rate = rospy.Rate(self.config['update_rate'])
while not rospy.is_shutdown():
if self.config['timeout'] > 0:
# If a timeout is set, zero the outputs to the thrusters if
# there is no command signal for the length of timeout
if rospy.Time.now(
) - self.last_update > self.config['timeout']:
print 'Turning thrusters off - inactive for too long'
if self.thrust is not None:
self.thrust.fill(0)
self.command_thrusters()
rate.sleep()
def setUpClass(cls):
parent_file_path = pathlib.Path(__file__).parent
thruster_manager_yaml = os.path.join(
str(parent_file_path),
'test_vehicle_thruster_manager_proportional.yaml')
xacro_file = os.path.join(str(parent_file_path),
'test_vehicle_x_axis.urdf.xacro')
doc = xacro.process(xacro_file)
# Initialize the ROS context for the test node
# FIXME Temp workaround TF for listener subscribing to relative topic
rclpy.init(args=[
'--ros-args', '--params-file', thruster_manager_yaml, '-r',
'__ns:=/test_vehicle', '-r', 'tf:=/tf', '-p',
'robot_description:=%s' % doc
])
# Name alias...why cls is not working ?
_class = TestThrusterManagerProportionalCorrect
_class._manager = ThrusterManager(
'test_thruster_manager_proportional_correct')
_class._thread = threading.Thread(target=rclpy.spin,
args=(_class._manager, ),
daemon=True)
_class._thread.start()
# Wait for the async initialization of the manager to finish
while not _class._manager.ready:
time.sleep(0.1)
def __init__(self, name, **kwargs):
"""Class constructor."""
ThrusterManager.__init__(self, name, **kwargs)
self.last_update = time_in_float_sec(self.get_clock().now())
self.input_sub = None
self.input_stamped_sub = None
self.thruster_info_service = None
self.curve_calc_service = None
self.set_thruster_manager_config_service = None
self.get_thruster_manager_config_service = None
self.timer_function = None
# Init the thruster allocator when the parent class has been initialized
# If the future is done, the callback is called immedialely
# NB: no need to lock
self.init_future.add_done_callback(self._init_class)
def __init__(self):
"""Class constructor."""
ThrusterManager.__init__(self)
self.last_update = rospy.Time.now()
# Subscriber to the wrench to be applied on the UUV
# This package's setup makes it so that the sub should respond to input
# coming from here, but we should probably replace this whole package
# with the controls team's package
self.input_sub = rospy.Subscriber('thruster_manager/input', Wrench,
self.input_callback)
# To deliver the wrench input with an option to use another body frame
# (options: base_link and base_link_ned), use the wrench stamped
# message
self.input_stamped_sub = rospy.Subscriber(
'thruster_manager/input_stamped', WrenchStamped,
self.input_stamped_callback)
self.thruster_info_service = rospy.Service(
'thruster_manager/get_thrusters_info', ThrusterManagerInfo,
self.get_thruster_info)
self.curve_calc_service = rospy.Service(
'thruster_manager/get_thruster_curve', GetThrusterCurve,
self.get_thruster_curve)
self.set_thruster_manager_config_service = rospy.Service(
'thruster_manager/set_config', SetThrusterManagerConfig,
self.set_config)
self.get_thruster_manager_config_service = rospy.Service(
'thruster_manager/get_config', GetThrusterManagerConfig,
self.get_config)
rate = rospy.Rate(self.config['update_rate'])
while not rospy.is_shutdown():
if self.config['timeout'] > 0:
# If a timeout is set, zero the outputs to the thrusters if
# there is no command signal for the length of timeout
if rospy.Time.now(
) - self.last_update > self.config['timeout']:
print 'Turning thrusters off - inactive for too long'
if self.thrust is not None:
self.thrust.fill(0)
self.command_thrusters()
rate.sleep()
def __init__(self):
"""Class constructor."""
ThrusterManager.__init__(self)
self.last_update = rospy.Time.now()
# Subscriber to the wrench to be applied on the UUV
self.input_sub = rospy.Subscriber('thruster_manager/input',
Wrench, self.input_callback)
# To deliver the wrench input with an option to use another body frame
# (options: base_link and base_link_ned), use the wrench stamped
# message
self.input_stamped_sub = rospy.Subscriber(
'thruster_manager/input_stamped', WrenchStamped,
self.input_stamped_callback)
self.thruster_info_service = rospy.Service(
'thruster_manager/get_thrusters_info', ThrusterManagerInfo,
self.get_thruster_info)
self.curve_calc_service = rospy.Service(
'thruster_manager/get_thruster_curve', GetThrusterCurve,
self.get_thruster_curve)
self.set_thruster_manager_config_service = rospy.Service(
'thruster_manager/set_config', SetThrusterManagerConfig,
self.set_config)
self.get_thruster_manager_config_service = rospy.Service(
'thruster_manager/get_config', GetThrusterManagerConfig,
self.get_config)
rate = rospy.Rate(self.config['update_rate'])
while not rospy.is_shutdown():
if self.config['timeout'] > 0:
# If a timeout is set, zero the outputs to the thrusters if
# there is no command signal for the length of timeout
if rospy.Time.now() - self.last_update > self.config['timeout']:
print 'Turning thrusters off - inactive for too long'
if self.thrust is not None:
self.thrust.fill(0)
self.command_thrusters()
rate.sleep()
def setUpClass(cls):
parent_file_path = pathlib.Path(__file__).parent
thruster_manager_yaml = os.path.join(
str(parent_file_path),
'test_vehicle_thruster_manager_proportional.yaml'
)
xacro_file = os.path.join(
str(parent_file_path),
'test_vehicle_x_axis.urdf.xacro'
)
output = os.path.join(
str(parent_file_path),
'robot_description_x_axis.urdf'
)
#TODO Change in foxy
if not pathlib.Path(output).exists():
doc = xacro.process(xacro_file)
try:
with open(output, 'w') as file_out:
file_out.write(doc)
except IOError as e:
print("Failed to open output:", exc=e)
# Initialize the ROS context for the test node
# FIXME Temp workaround TF for listener subscribing to relative topic
rclpy.init(args=['--ros-args', '--params-file', thruster_manager_yaml,
'-r', '__ns:=/test_vehicle', '-r', 'tf:=/tf', '-p', 'urdf_file:=%s' % output])
# Name alias...why cls is not working ?
_class = TestThrusterManagerProportionalCorrect
_class._manager = ThrusterManager('test_thruster_manager_proportional_correct')
_class._thread = threading.Thread(target=rclpy.spin, args=(_class._manager,), daemon=True)
_class._thread.start()
# Wait for the async initialization of the manager to finish
while not _class._manager.ready:
time.sleep(0.1)
# distributed under the License is distributed on an "AS IS" BASIS,
# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
# See the License for the specific language governing permissions and
# limitations under the License.
import rospy
import roslib
import unittest
import numpy as np
from uuv_thrusters import ThrusterManager
PKG = 'uuv_thruster_manager'
roslib.load_manifest(PKG)
rospy.init_node('test_thruster_manager_proportional_correct', anonymous=True)
MANAGER = ThrusterManager()
REFERENCE_TAM = np.array([
[1, 0, 0, 0, 0, 0],
[0.87758256, 0, -0.47942554, 0.47942554, 0.47942554, 0.87758256],
[0.87758256, 0.47942554, 0, -0.47942554, 0.87758256, -0.87758256]
]).T
class TestThrusterManagerProportionalCorrect(unittest.TestCase):
def test_initialization(self):
self.assertEqual(MANAGER.namespace, '/test_vehicle/')
self.assertEqual(MANAGER.config['tf_prefix'], 'test_vehicle')
self.assertEqual(MANAGER.config['base_link'], 'base_link')
self.assertEqual(MANAGER.config['thruster_topic_prefix'], 'thrusters/')
self.assertEqual(MANAGER.config['thruster_frame_base'], 'thruster_')