A simple ROS node to move SCUTTLE to a goal

move_scuttle.py

#!/usr/bin/env python

from math import pow, atan2, sqrt, copysign

import rospy
from geometry_msgs.msg import Twist, Pose, PoseWithCovariance
from nav_msgs.msg import Odometry
from tf.transformations import euler_from_quaternion, quaternion_from_euler

class ScuttleControl:

    def __init__(self):
        self.velocity_publisher = rospy.Publisher('/cmd_vel', Twist, queue_size=10)

        self.pose_subscriber = rospy.Subscriber('/odom', Odometry, self.update_pose)

        self.pose = Pose()
        self.rate_in_hz = 10
        self.rate = rospy.Rate(self.rate_in_hz)
        self.max_linear_velocity = 0.45
        self.max_linear_acceleration = 0.3
        self.max_angular_velocity = 2.7
        self.max_angular_acceleration = 1.2

    def update_pose(self, data):
        pose = data.pose.pose
        orientation = pose.orientation
        position = pose.position
        twist = data.twist.twist

        orientation_list = [orientation.x, orientation.y, orientation.z, orientation.w]
        (roll, pitch, yaw) = euler_from_quaternion(orientation_list)

        self.x = position.x
        self.y = position.y
        self.yaw = yaw

        self.vx = twist.linear.x
        self.vyaw = twist.angular.z

    def velocity_with_ramp(self, current_velocity, desired_velocity, acceleration):
        if desired_velocity == current_velocity:
            return desired_velocity
        else:
            if desired_velocity > current_velocity:
                # accelerating
                achievable_velocity = current_velocity + acceleration / self.rate_in_hz
                if achievable_velocity > desired_velocity:
                    # desired acceleration is less than the possible acceleration
                    return desired_velocity
                else:
                    # desired acceleration is more than the possible acceleration
                    return achievable_velocity
            else:
                achievable_velocity = current_velocity - acceleration / self.rate_in_hz
                if achievable_velocity < desired_velocity:
                    # desired deceleration is less than the possible deceleration
                    return desired_velocity
                else:
                    # desired deceleration is more than the possible decelaration
                    return achievable_velocity

    def distance(self, pose):
        dx = pose.position.x - self.x
        dy = pose.position.y - self.y
        return sqrt(pow(dx, 2) + pow(dy, 2))

    def linear_vel(self, pose, constant=0.5):
        # We want to drive at this velocity
        desired_velocity = constant * self.distance(pose)

        # But we have maximum accelerations and deccellerations
        # If we don't have those SCUTTLE will stand on its rear wheels (which
        # isn't possible in real life)
        velocity = self.velocity_with_ramp(self.vx, desired_velocity, self.max_linear_acceleration)
        if abs(velocity) > self.max_linear_velocity:
            return copysign(self.max_linear_velocity, velocity)
        else:
            return velocity

    def steering_angle(self, pose):
        angle = atan2(pose.position.y - self.y, pose.position.x - self.x)

    def angular_vel(self, pose, constant=6):
        desired_angular_velocity = constant * (self.steering_angle(pose) - self.yaw)
        # This really should have an acceleration and decceleration too

        angular_velocity = self.velocity_with_ramp(self.vyaw, desired_angular_velocity, self.max_angular_acceleration)
        if abs(angular_velocity) > self.max_angular_velocity:
            return copysign(self.max_angular_velocity, angular_velocity)
        else:
            return angular_velocity

    def move2goal(self, x, y, tol):
        goal_pose = Pose()

        # get input from the user
        goal_pose.position.x = x
        goal_pose.position.y = y

        distance_tolerance = tol

        vel_msg = Twist()

        current_distance = self.distance(goal_pose)
        print(f'current distance: {current_distance}')

        while current_distance >= distance_tolerance:
            linear_velocity = self.linear_vel(goal_pose)
            angular_velocity = self.angular_vel(goal_pose)

            print(f'current distance: {current_distance}. linear velocity: {linear_velocity} - angular velocity: {angular_velocity}')

            vel_msg.linear.x = linear_velocity
            vel_msg.linear.y = 0
            vel_msg.linear.z = 0

            vel_msg.angular.x = 0
            vel_msg.angular.y = 0
            vel_msg.angular.z = angular_velocity

            self.velocity_publisher.publish(vel_msg)

            self.rate.sleep()

            print(f'New pose [x: {self.x}, y: {self.y}, rotation: {self.yaw}]')
            current_distance = self.distance(goal_pose)

        print(f'Goal reached. Stopping ...')
        vel_msg.linear.x = 0
        vel_msg.angular.z = 0
        self.velocity_publisher.publish(vel_msg)

    def stop(self):
        print(f'Emergency stop!')
        vel_msg = Twist()
        vel_msg.linear.x = 0
        vel_msg.angular.z = 0
        self.velocity_publisher.publish(vel_msg)

if __name__ == '__main__':
    # Anonymous -> give each node an unique ID
    rospy.init_node('scuttle_control', anonymous=True)

    bot = ScuttleControl()
    try:
        bot.move2goal()

        # if we press ctrl + C the program stops
        rospy.spin()
    except rospy.ROSInterruptException:
        bot.stop()
        pass

move_scuttle_on_trajectory.py

#!/usr/bin/env python

from math import pow, atan2, sqrt, copysign

import rospy
from geometry_msgs.msg import Twist, Pose, PoseWithCovariance
from nav_msgs.msg import Odometry
from tf.transformations import euler_from_quaternion, quaternion_from_euler
from move_scuttle import ScuttleControl

class ScuttleTrajectory:

    def __init__(self):
        self.control = ScuttleControl()

    def move2goal(self):

        coordinates = []
        while True:
            waypoint = input("Set your x,y marker: ")
            if not waypoint:
                break

            coordinate = waypoint.split(',')
            x = float(coordinate[0])
            y = float(coordinate[1])

            coordinates.append([x, y])


        # get input from the user
        for coordinate in coordinates:
            self.control.move2goal(coordinate[0], coordinate[1], 0.25)

        print(f'final waypoint reached. Stopping ...')

        # if we press ctrl + C the program stops
        rospy.spin()

    def stop(self):
        print(f'Emergency stop!')
        self.control.stop()

if __name__ == '__main__':
    # Anonymous -> give each node an unique ID
    rospy.init_node('scuttle_trajectory', anonymous=True)
    bot = ScuttleTrajectory()
    try:
        bot.move2goal()
    except rospy.ROSInterruptException:
        bot.stop()
        pass