コード例 #1
0
    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))
コード例 #2
0
    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
コード例 #3
0
ファイル: listener.py プロジェクト: ErolB/Sub8
    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
コード例 #4
0
 def clear(self):
     yield self.set_kill(
         SetKillRequest(
             kill=Kill(
                 header=Header(stamp=self._nh.get_time(), ),
                 id=self.id,
             ),
             clear=True,
         ))
コード例 #5
0
 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()
コード例 #6
0
 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()
コード例 #7
0
    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)
コード例 #8
0
 def clear(self):
     self.set_kill(
         Kill(
             header=Header(stamp=rospy.Time.now(), ),
             id=self.id,
         ), True)