A ROS node that sends velocity commands through MavROS using a joy

joy_control.py

#!/usr/bin/python3
'''
MIT License
Copyright (c) 2022 Michail Kalaitzakis
Permission is hereby granted, free of charge, to any person obtaining a copy
of this software and associated documentation files (the "Software"), to deal
in the Software without restriction, including without limitation the rights
to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
copies of the Software, and to permit persons to whom the Software is
furnished to do so, subject to the following conditions:
The above copyright notice and this permission notice shall be included in all
copies or substantial portions of the Software.
THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
SOFTWARE.
'''
import threading

import rospy as rp

from mavros_msgs.srv import SetMode

from sensor_msgs.msg import Joy
from mavros_msgs.msg import PositionTarget, State


class JoyRCNode():
    '''
     A ROS node that sends velocity commands through MavROS using a joy
    '''

    # Maximum allowed velocities
    _HOR_VEL_MAX = 1.0
    _VER_VEL_MAX = 1.0
    _YAW_RATE_MAX = 1.0

    # Joy axes map to velocities
    _X_VEL_AXIS = 3
    _Y_VEL_AXIS = 2
    _Z_VEL_AXIS = 1
    _YAW_RATE_AXIS = 0

    # Joy button for offboard
    _OFFBOARD_BUTTON = 6

    def __init__(self, rate):
        rp.init_node('joy_rc_node')

        self.rate = rate

        # Check that services are available
        rp.loginfo('Checking that services are available')
        rp.wait_for_service('/mavros/set_mode')
        rp.loginfo('MavROS services are available')

        # Velocity cmds
        self.vel_x_cmd = 0.0
        self.vel_y_cmd = 0.0
        self.vel_z_cmd = 0.0
        self.vel_q_cmd = 0.0

        # Control switches
        self.ofb_bttn = 0

        # Status
        self.connected = False
        self.armed = False
        self.mode = None

        # Service clients
        self.set_mode_serv = rp.ServiceProxy('/mavros/set_mode', SetMode)

        # Subscribers
        self.state_sub = rp.Subscriber(
            '/mavros/state', State, self.stateCallback, queue_size=1)
        self.joy_sub = rp.Subscriber(
            '/joy', Joy, self.joyCallback, queue_size=1)

        # Publishers
        self.vel_cmd_pub = rp.Publisher(
            '/mavros/setpoint_raw/local', PositionTarget, queue_size=1)

        t = threading.Thread(target=self.commandPublisher)
        t.start()

        rp.spin()

    def stateCallback(self, msg):
        self.connected = msg.connected
        self.armed = msg.armed
        self.mode = msg.mode

    def joyCallback(self, msg):
        self.vel_x_cmd = self._HOR_VEL_MAX * msg.axes[self._X_VEL_AXIS]
        self.vel_y_cmd = self._HOR_VEL_MAX * msg.axes[self._Y_VEL_AXIS]
        self.vel_z_cmd = self._VER_VEL_MAX * msg.axes[self._Z_VEL_AXIS]
        self.vel_q_cmd = self._YAW_RATE_MAX * msg.axes[self._YAW_RATE_AXIS]

        self.ofb_bttn = msg.buttons[self._OFFBOARD_BUTTON]

    def commandPublisher(self,):
        r = rp.Rate(self.rate)
        cmd_msg = PositionTarget()
        cmd_msg.coordinate_frame = PositionTarget().FRAME_BODY_NED
        cmd_msg.type_mask = \
            PositionTarget().IGNORE_PX + \
            PositionTarget().IGNORE_PY + \
            PositionTarget().IGNORE_PZ + \
            PositionTarget().IGNORE_AFX + \
            PositionTarget().IGNORE_AFY + \
            PositionTarget().IGNORE_AFZ + \
            PositionTarget().IGNORE_YAW

        while not rp.is_shutdown():
            if self.connected:
                cmd_msg.velocity.x = 0.0
                cmd_msg.velocity.y = 0.0
                cmd_msg.velocity.z = 0.0
                cmd_msg.yaw_rate = 0.0

                # Make sure we are in the correct flight mode
                if self.ofb_bttn == 0.0 and self.mode != 'POSCTL':
                    self.set_mode_serv(0, 'POSCTL')
                elif self.ofb_bttn == 1.0 and self.mode != 'OFFBOARD':
                    self.set_mode_serv(0, 'OFFBOARD')

                # Only update values if offboard button is held
                if self.armed and self.ofb_bttn == 1.0:
                    cmd_msg.velocity.x = self.vel_x_cmd
                    cmd_msg.velocity.y = self.vel_y_cmd
                    cmd_msg.velocity.z = self.vel_z_cmd
                    cmd_msg.yaw_rate = self.vel_q_cmd

                # Always send a cmd so that mode changes are allowed
                cmd_msg.header.stamp = rp.Time.now()
                self.vel_cmd_pub.publish(cmd_msg)

            else:
                rp.loginfo('Vehicle not connected')

            r.sleep()


if __name__ == '__main__':
    JoyRCNode(30)