def set_model_pose(self, pose, twist=None, model='sub8'):
'''
Set the position of 'model' to 'pose'.
It may be helpful to kill the sub before moving it.
TODO:
- Deprecate kill stuff
'''
set_state = yield self.nh.get_service_client('/gazebo/set_model_state',
SetModelState)
if twist is None:
twist = numpy_to_twist([0, 0, 0], [0, 0, 0])
model_state = ModelState()
model_state.model_name = model
model_state.pose = pose
model_state.twist = twist
if model == 'sub8':
# TODO: Deprecate kill stuff (Zach's PR upcoming)
kill = self.nh.get_service_client('/set_kill', SetKill)
yield kill(SetKillRequest(kill=Kill(id='initial', active=False)))
yield kill(SetKillRequest(kill=Kill(active=True)))
yield self.nh.sleep(.1)
yield set_state(SetModelStateRequest(model_state))
yield self.nh.sleep(.1)
yield kill(SetKillRequest(kill=Kill(active=False)))
else:
set_state(SetModelStateRequest(model_state))
def clear(self):
yield self.set_kill(
SetKillRequest(
kill=Kill(
header=Header(stamp=self._nh.get_time(), ),
id=self.id,
),
clear=True,
))
def send(self, active):
try:
yield self.set_kill(
SetKillRequest(
kill=Kill(
header=Header(stamp=self._nh.get_time(), ),
id=self.id,
active=active,
description=self.description,
),
clear=False,
))
except:
import traceback
traceback.print_exc()