You can not select more than 25 topics
Topics must start with a letter or number, can include dashes ('-') and can be up to 35 characters long.
38 lines
1.0 KiB
38 lines
1.0 KiB
import rclpy
|
|
from rclpy.node import Node
|
|
from geometry_msgs.msg import Twist
|
|
import time
|
|
|
|
class TShapeControl(Node):
|
|
def __init__(self):
|
|
super().__init__('t_shape_control')
|
|
self.publisher = self.create_publisher(Twist, '/cmd_vel', 10)
|
|
|
|
def move(self, linear=0.0, angular=0.0, duration=2.0):
|
|
twist = Twist()
|
|
twist.linear.x = linear
|
|
twist.angular.z = angular
|
|
end_time = time.time() + duration
|
|
while time.time() < end_time:
|
|
self.publisher.publish(twist)
|
|
time.sleep(0.1)
|
|
|
|
def run(self):
|
|
while rclpy.ok():
|
|
self.move(0.2, 0.0, 3.0) # 前进
|
|
self.move(0.0, 0.5, 2.0) # 左转
|
|
self.move(0.2, 0.0, 3.0) # 前进
|
|
self.move(-0.2, 0.0, 3.0) # 回退
|
|
self.move(0.0, -0.5, 2.0) # 右转
|
|
self.move(0.2, 0.0, 3.0) # 前进
|
|
|
|
def main(args=None):
|
|
rclpy.init(args=args)
|
|
node = TShapeControl()
|
|
node.run()
|
|
node.destroy_node()
|
|
rclpy.shutdown()
|
|
|
|
if __name__ == '__main__':
|
|
main()
|