Skip to content

Instantly share code, notes, and snippets.

Embed
What would you like to do?
A simple ROS node to move SCUTTLE to a goal
#!/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
#!/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
Sign up for free to join this conversation on GitHub. Already have an account? Sign in to comment