Script to act as a man-in-the-middle for TwistStamped messages to apply gains and max values to linear.x and angular.z (Used for increasing/decreasing values sent to a car for safety and fixing of bugs. Our car needed a 25x gain on angular.z to turn).

twist_gains.py

#!/usr/bin/env python

import math
import rospy
from geometry_msgs.msg import TwistStamped
# Needs ros-DISTRO-ddynamic-reconfigure-python
from ddynamic_reconfigure_python.ddynamic_reconfigure import DDynamicReconfigure

"""
Class that acts as a man-in-the-middle node
getting a TwistStamped in and
outputing a TwistStamped out
that has been applied a set of gains
and a set of cut off max values
that can be configured via Dynamic Reconfigure.
(with rosrun rqt_reconfigure rqt_reconfigure).

Author: Sammy Pfeiffer <Sammy.Pfeiffer@student.uts.edu.au>
"""


class TwistStampedGains(object):
    def __init__(self):
        # Out topic
        self.pub = rospy.Publisher('/twist_raw',
                                   TwistStamped, queue_size=1)

        self.ddr = DDynamicReconfigure('twist_gains')
        self.ddr.add_variable('linear_x_gain',
                              "gain for twist.linear.x",
                              1.0, min=-5.0, max=5.0)
        self.ddr.add_variable('angular_z_gain',
                              "gain for twist.angular.z",
                              25.0, min=0.0, max=100.0)
        self.ddr.add_variable("max_linear_x", "Max +- linear.x (m/s)",
                              5.5, min=0.0, max=55.0)
        self.ddr.add_variable("max_angular_z", "Max +- angular.z (rad/s)",
                              6.0, min=0.0, max=math.radians(360.0 * 10.0))
        self.ddr.start(self.dyn_rec_callback)

        # In topic
        self.sub = rospy.Subscriber('/twist_raw_tmp',
                                    TwistStamped, self._cb,
                                    queue_size=1)
        rospy.loginfo("Node " + rospy.get_name() + " initialized.")
        rospy.loginfo("Listening to topic: " + str(self.sub.resolved_name))
        rospy.loginfo("Publishing on topic: " + str(self.pub.resolved_name))
        rospy.loginfo("Applying gains: x: " + str(self.linear_x_gain) +
                      " z: " + str(self.angular_z_gain))
        rospy.loginfo("Applying max values: x: " + str(self.max_x) +
                      " z: " + str(self.max_z))
        rospy.loginfo(
            "Tune these values by using: rosrun rqt_reconfigure rqt_reconfigure")

    def dyn_rec_callback(self, config, level):
        self.linear_x_gain = config['linear_x_gain']
        self.angular_z_gain = config['angular_z_gain']
        self.max_x = config['max_linear_x']
        self.max_z = config['max_angular_z']
        rospy.loginfo(rospy.get_name() +
                      " got a reconfigure callback: " +
                      str(config['groups']['parameters']))
        return config

    def _cb(self, msg):
        # Apply gains
        msg.twist.linear.x *= self.linear_x_gain
        msg.twist.angular.z *= self.angular_z_gain

        # Apply max values
        if abs(msg.twist.linear.x) > self.max_x:
            if msg.twist.linear.x >= 0.0:
                msg.twist.linear.x = self.max_x
            else:
                msg.twist.linear.x = - self.max_z
        if abs(msg.twist.angular.z) > self.max_z:
            if msg.twist.angular.z >= 0.0:
                msg.twist.angular.z = self.max_z
            else:
                msg.twist.angular.z = - self.max_z

        self.pub.publish(msg)


if __name__ == '__main__':
    rospy.init_node('twist_gains')
    tsg = TwistStampedGains()
    rospy.spin()