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 get_killed(self):
# Check if the kill_master is publishing
if self._sub.get_num_connections() == 0:
# Kill master isn't publishing
k = Kill()
k.id = 'This kill listener'
k.active = True
k.description = 'Kill master is not publishing'
self._kills = [k]
return True
if self._kills is not None:
return len(self.get_kills()) > 0
else:
return True
def get_killed(self):
# Check if the kill_master is publishing
if self._sub.get_num_connections() == 0:
# Kill master isn't publishing
k = Kill()
k.id = 'This kill listener'
k.active = True
k.description = 'Kill master is not publishing'
self._kills = [k]
return True
if self._kills is not None:
return len(self.get_kills()) > 0
else:
return True
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:
self.set_kill(
Kill(
header=Header(stamp=rospy.Time.now(), ),
id=self.id,
active=active,
description=self.description,
), False)
except:
import traceback
traceback.print_exc()
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()
def __init__(self, nh):
self.kills_variable = variable.Variable([
Kill(header=Header(stamp=nh.get_time()),
id='null',
active=True,
description=
'default kill until a kill update message is received')
])
self.killed_variable = variable.Variable(
kills_to_killed(self.kills_variable.value))
self.kills_variable.changed.watch(
lambda kills: self.killed_variable.set(kills_to_killed(kills)))
def _killmsg_callback(msg):
self.kills_variable.set([k for k in msg.kills if k.active])
self._sub = nh.subscribe('/kill', KillsStamped, _killmsg_callback)
def clear(self):
self.set_kill(
Kill(
header=Header(stamp=rospy.Time.now(), ),
id=self.id,
), True)