71 lines
1.7 KiB
Python
71 lines
1.7 KiB
Python
import rclpy
|
|
from rclpy.node import Node
|
|
from geometry_msgs.msg import Twist
|
|
|
|
from turtlesim.srv import TeleportAbsolute
|
|
from turtlesim.srv import SetPen
|
|
|
|
delta_t = 0.1
|
|
v = 0.5
|
|
t = 0
|
|
|
|
|
|
def robot_angular_v(tt):
|
|
return tt / 100
|
|
|
|
|
|
def timed_callback():
|
|
global t, node, pub
|
|
t += delta_t
|
|
|
|
w = robot_angular_v(t)
|
|
|
|
msg = Twist()
|
|
msg.linear.x = v
|
|
msg.angular.z = w
|
|
pub.publish(msg)
|
|
|
|
|
|
def main(args=None):
|
|
rclpy.init(args=args)
|
|
global node, pub
|
|
|
|
node = Node('my_favourite_turtle')
|
|
|
|
cli = node.create_client(SetPen, "/turtle1/set_pen")
|
|
req = SetPen.Request()
|
|
req.off = True
|
|
while not cli.wait_for_service(timeout_sec=1.0):
|
|
node.get_logger().info('1: set_pen service not available, waiting again...')
|
|
future = cli.call_async(req)
|
|
rclpy.spin_until_future_complete(node, future)
|
|
|
|
cli = node.create_client(TeleportAbsolute, "/turtle1/teleport_absolute")
|
|
req = TeleportAbsolute.Request()
|
|
req.x = 0.0
|
|
req.y = 0.0
|
|
req.theta = 0.0
|
|
while not cli.wait_for_service(timeout_sec=1.0):
|
|
node.get_logger().info('2: teleport_absolute service not available, waiting again...')
|
|
future = cli.call_async(req)
|
|
rclpy.spin_until_future_complete(node, future)
|
|
|
|
cli = node.create_client(SetPen, "/turtle1/set_pen")
|
|
req = SetPen.Request()
|
|
req.off = False
|
|
req.r = 255
|
|
while not cli.wait_for_service(timeout_sec=1.0):
|
|
node.get_logger().info('3: set_pen service not available, waiting again...')
|
|
future = cli.call_async(req)
|
|
rclpy.spin_until_future_complete(node, future)
|
|
|
|
pub = node.create_publisher(Twist, '/turtle1/cmd_vel', 10)
|
|
node.create_timer(delta_t, timed_callback)
|
|
|
|
rclpy.spin(node)
|
|
rclpy.shutdown()
|
|
|
|
|
|
if __name__ == '__main__':
|
|
main()
|