Exemple #1
0
    def request_wrench_cb(self, msg):
        '''Callback for requesting a wrench'''
        time_now = rospy.Time.now()
        if (time_now - self.last_command_time) < self._min_command_time:
            return
        else:
            self.last_command_time = rospy.Time.now()

        force = rosmsg_to_numpy(msg.wrench.force)
        torque = rosmsg_to_numpy(msg.wrench.torque)
        wrench = np.hstack([force, torque])

        success = False
        while not success:
            u, success = self.map(wrench)
            if not success:
                # If we fail to compute, shrink the wrench
                wrench = wrench * 0.75
                continue

            thrust_cmds = []
            # Assemble the list of thrust commands to send
            for name, thrust in zip(self.thruster_name_map, u):
                # > Can speed this up by avoiding appends
                if name in self.dropped_thrusters:
                    continue  # Ignore dropped thrusters

                if np.fabs(thrust) < self.min_commandable_thrust:
                    thrust = 0
                thrust_cmds.append(ThrusterCmd(name=name, thrust=thrust))

        # If the sub is not killed
        if self.killed is False:
            self.thruster_pub.publish(thrust_cmds)
Exemple #2
0
    def request_wrench_cb(self, msg):
        '''Callback for requesting a wrench'''
        time_now = rospy.Time.now()
        if (time_now - self.last_command_time) < self._min_command_time:
            return
        else:
            self.last_command_time = rospy.Time.now()

        force = rosmsg_to_numpy(msg.wrench.force)
        torque = rosmsg_to_numpy(msg.wrench.torque)
        wrench = np.hstack([force, torque])

        success = False
        while not success:
            u, success = self.map(wrench)
            if not success:
                # If we fail to compute, shrink the wrench
                wrench = wrench * 0.75
                continue

            thrust_cmds = []
            # Assemble the list of thrust commands to send
            for name, thrust in zip(self.thruster_name_map, u):
                # > Can speed this up by avoiding appends
                if name in self.dropped_thrusters:
                    continue  # Ignore dropped thrusters

                if np.fabs(thrust) < self.min_commandable_thrust:
                    thrust = 0
                thrust_cmds.append(ThrusterCmd(name=name, thrust=thrust))

        # If the sub is not killed
        if self.killed is False:
            self.thruster_pub.publish(thrust_cmds)
Exemple #3
0
    def twist_cb(self, msg):
        if self.cur_orientation is None or self.cur_position is None:
            return
        linear = sub8_utils.rosmsg_to_numpy(msg.linear)
        angular = sub8_utils.rosmsg_to_numpy(msg.angular)
        self.target_position += self.target_orientation.dot(self._position_gain * linear)
        self.diff_position = np.subtract(self.cur_position, self.target_position)

        if self.mode == '2d':
            # Ignore 6-dof shenanigans
            angular[0] = 0
            angular[1] = 0

        gained_angular = self._orientation_gain * angular
        skewed = sub8_utils.skew_symmetric_cross(gained_angular)
        rotation = linalg.expm(skewed)

        # TODO: Better
        # self.target_orientation = self.cur_orientation
        # self.target_orientation = rotation.dot(self.target_orientation)

        self.target_orientation = self.target_orientation.dot(rotation)
        self.target_distance = round(np.linalg.norm(np.array([self.diff_position[0], self.diff_position[1]])), 3)
        self.target_depth = round(self.diff_position[2], 3)
        self.target_orientation = self.target_orientation.dot(rotation)

        blank = np.eye(4)
        blank[:3, :3] = self.target_orientation
        self.target_orientation_quaternion = tf.transformations.quaternion_from_matrix(blank)
        self.publish_target_pose(self.target_position, self.target_orientation_quaternion)
Exemple #4
0
    def test_make_wrench_stamped(self):
        '''Test that wrenchmaking works'''
        for k in range(10):
            force = np.random.random(3) * 10
            torque = np.random.random(3) * 10
        wrench_msg = make_wrench_stamped(force, torque, frame='/enu')

        msg_force = rosmsg_to_numpy(wrench_msg.wrench.force)
        msg_torque = rosmsg_to_numpy(wrench_msg.wrench.torque)
Exemple #5
0
    def test_make_wrench_stamped(self):
        '''Test that wrenchmaking works'''
        for k in range(10):
            force = np.random.random(3) * 10
            torque = np.random.random(3) * 10
        wrench_msg = make_wrench_stamped(force, torque, frame='/enu')

        msg_force = rosmsg_to_numpy(wrench_msg.wrench.force)
        msg_torque = rosmsg_to_numpy(wrench_msg.wrench.torque)
Exemple #6
0
    def twist_cb(self, msg):
        linear = sub8_utils.rosmsg_to_numpy(msg.linear)
        angular = sub8_utils.rosmsg_to_numpy(msg.angular)

        if self.behavior == 'wrench':
            # Directly transcribe linear and angular 'velocities' to torque
            # TODO: Setup actual TF!

            self.wrench_pub.publish(
                sub8_utils.make_wrench_stamped(
                    force=self.linear_gain * self.world_to_body.dot(linear),
                    torque=self.angular_gain * self.world_to_body.dot(angular)
                )
            )
        else:
            raise(NotImplementedError("Velocity and position control by mouse are not yet supported"))
Exemple #7
0
    def test_rosmsg_to_numpy_quaternion(self):
        '''Test a rosmsg conversion for a geometry_msgs/Quaternion'''
        # I know this is not a unit quaternion
        q = Quaternion(x=0.7, y=0.7, z=0.1, w=0.2)
        numpy_array = rosmsg_to_numpy(q)

        np.testing.assert_allclose(np.array([0.7, 0.7, 0.1, 0.2]), numpy_array)
Exemple #8
0
    def request_wrench_cb(self, msg):
        '''Callback for requesting a wrench'''
        force = rosmsg_to_numpy(msg.wrench.force)
        torque = rosmsg_to_numpy(msg.wrench.torque)
        wrench = np.hstack([force, torque])

        u, success = self.map(wrench)
        if success:
            thrust_cmds = []
            # Assemble the list of thrust commands to send
            for name, thrust in zip(self.thruster_name_map, u):
                # > Can speed this up by avoiding appends
                thrust_cmds.append(ThrusterCmd(name=name, thrust=thrust))
            self.thruster_pub.publish(thrust_cmds)
        else:
            # I expect this to happen frequently enough that we should not raise
            rospy.logwarn("Could not achieve wrench of {}".format(wrench))
Exemple #9
0
    def request_wrench_cb(self, msg):
        '''Callback for requesting a wrench'''
        force = rosmsg_to_numpy(msg.wrench.force)
        torque = rosmsg_to_numpy(msg.wrench.torque)
        wrench = np.hstack([force, torque])

        u, success = self.map(wrench)
        if success:
            thrust_cmds = []
            # Assemble the list of thrust commands to send
            for name, thrust in zip(self.thruster_name_map, u):
                # > Can speed this up by avoiding appends
                thrust_cmds.append(ThrusterCmd(name=name, thrust=thrust))
            self.thruster_pub.publish(thrust_cmds)
        else:
            # I expect this to happen frequently enough that we should not raise
            rospy.logwarn("Could not achieve wrench of {}".format(wrench))
Exemple #10
0
    def twist_cb(self, msg):
        linear = sub8_utils.rosmsg_to_numpy(msg.linear)
        angular = sub8_utils.rosmsg_to_numpy(msg.angular)

        if self.behavior == 'wrench':
            # Directly transcribe linear and angular 'velocities' to torque
            # TODO: Setup actual TF!

            self.wrench_pub.publish(
                sub8_utils.make_wrench_stamped(
                    force=self.linear_gain * self.world_to_body.dot(linear),
                    torque=self.angular_gain *
                    self.world_to_body.dot(angular)))
        else:
            raise (NotImplementedError(
                "Velocity and position control by mouse are not yet supported")
                   )
Exemple #11
0
    def test_rosmsg_to_numpy_custom(self):
        '''Test a rosmsg conversion for a custom msg'''
        pose_2d = Pose2D(x=1.0, y=2.0, theta=3.14)
        numpy_array = rosmsg_to_numpy(pose_2d, ['x', 'y', 'theta'])

        np.testing.assert_allclose(
            np.array([1.0, 2.0, 3.14]),
            numpy_array
        )
Exemple #12
0
    def test_rosmsg_to_numpy_vector(self):
        '''Test a rosmsg conversion for a geometry_msgs/Vector'''
        v = Vector3(x=0.1, y=99., z=21.)
        numpy_array = rosmsg_to_numpy(v)

        np.testing.assert_allclose(
            np.array([0.1, 99., 21.]),
            numpy_array
        )
Exemple #13
0
    def test_rosmsg_to_numpy_quaternion(self):
        '''Test a rosmsg conversion for a geometry_msgs/Quaternion'''
        # I know this is not a unit quaternion
        q = Quaternion(x=0.7, y=0.7, z=0.1, w=0.2)
        numpy_array = rosmsg_to_numpy(q)

        np.testing.assert_allclose(
            np.array([0.7, 0.7, 0.1, 0.2]),
            numpy_array
        )
Exemple #14
0
    def twist_cb(self, msg):
        if self.cur_orientation is None or self.cur_position is None:
            return
        linear = sub8_utils.rosmsg_to_numpy(msg.linear)
        angular = sub8_utils.rosmsg_to_numpy(msg.angular)
        self.target_position += self.target_orientation.dot(self._position_gain * linear)

        gained_angular = self._orientation_gain * angular
        skewed = sub8_utils.skew_symmetric_cross(gained_angular)
        rotation = linalg.expm(skewed)

        # TODO: Better
        # self.target_orientation = self.cur_orientation
        # self.target_orientation = rotation.dot(self.target_orientation)
        self.target_orientation = self.target_orientation.dot(rotation)

        blank = np.eye(4)
        blank[:3, :3] = self.target_orientation
        self.target_orientation_quaternion = tf.transformations.quaternion_from_matrix(blank)
        self.publish_target_pose(self.target_position, self.target_orientation_quaternion)
Exemple #15
0
    def twist_cb(self, msg):
        if self.cur_orientation is None or self.cur_position is None:
            return
        linear = sub8_utils.rosmsg_to_numpy(msg.linear)
        angular = sub8_utils.rosmsg_to_numpy(msg.angular)
        self.target_position += self.target_orientation.dot(
            self._position_gain * linear)

        gained_angular = self._orientation_gain * angular
        skewed = sub8_utils.skew_symmetric_cross(gained_angular)
        rotation = linalg.expm(skewed)

        # TODO: Better
        # self.target_orientation = self.cur_orientation
        # self.target_orientation = rotation.dot(self.target_orientation)
        self.target_orientation = self.target_orientation.dot(rotation)

        blank = np.eye(4)
        blank[:3, :3] = self.target_orientation
        self.target_orientation_quaternion = tf.transformations.quaternion_from_matrix(
            blank)
        self.publish_target_pose(self.target_position,
                                 self.target_orientation_quaternion)
Exemple #16
0
    def set_pose_server(self, srv):
        '''Set the pose of the submarine
        TODO: Make this an 'abstract' method of Entity, and assign each new Entity a name/id
        '''
        rospy.logwarn("Manually setting position of simulated vehicle")
        position = sub8_utils.rosmsg_to_numpy(srv.pose.position)
        self.body.setPosition(position)

        rotation_q = sub8_utils.rosmsg_to_numpy(srv.pose.orientation)
        rotation_norm = np.linalg.norm(rotation_q)

        velocity = sub8_utils.rosmsg_to_numpy(srv.twist.linear)
        angular = sub8_utils.rosmsg_to_numpy(srv.twist.angular)

        self.body.setLinearVel(velocity)
        self.body.setAngularVel(angular)

        # If the commanded rotation is valid
        if np.isclose(rotation_norm, 1., atol=1e-2):
            self.body.setQuaternion(sub8_utils.normalize(rotation_q))
        else:
            rospy.logwarn("Commanded quaternion was not a unit quaternion, NOT setting rotation")

        return SimSetPoseResponse()
Exemple #17
0
def run(sub):
    yield txros.util.wall_sleep(1.0)

    # == MISSION CODE HERE ===================
    ret = yield missions.align_channel.run(sub)
    if ret is None:
        print "F**k"

    odom = yield sub.last_pose()
    rotation = rosmsg_to_numpy(odom.pose.pose.orientation)

    # Buoys are around 2m from the ground?
    #yield sub.to_height(2)

    yield missions.buoy.run(sub)

    yield sub.move.backward(1).go()
    yield sub.move.up(1.5).go()
    yield sub.move.set_orientation(rotation).zero_roll_and_pitch().go()

    yield sub.move.forward(5)

    print "DONE"
Exemple #18
0
 def from_Pose(cls, frame_id, msg):
     return cls(frame_id, rosmsg_to_numpy(msg.position), rosmsg_to_numpy(msg.orientation))
Exemple #19
0
 def from_Pose(cls, frame_id, msg):
     return cls(frame_id, rosmsg_to_numpy(msg.position), rosmsg_to_numpy(msg.orientation))
Exemple #20
0
    def test_rosmsg_to_numpy_vector(self):
        '''Test a rosmsg conversion for a geometry_msgs/Vector'''
        v = Vector3(x=0.1, y=99., z=21.)
        numpy_array = rosmsg_to_numpy(v)

        np.testing.assert_allclose(np.array([0.1, 99., 21.]), numpy_array)
Exemple #21
0
    def test_rosmsg_to_numpy_custom(self):
        '''Test a rosmsg conversion for a custom msg'''
        pose_2d = Pose2D(x=1.0, y=2.0, theta=3.14)
        numpy_array = rosmsg_to_numpy(pose_2d, ['x', 'y', 'theta'])

        np.testing.assert_allclose(np.array([1.0, 2.0, 3.14]), numpy_array)