diff --git a/src/assignment_9/src/exercise1_subscriber.py b/src/assignment_9/src/exercise1_subscriber.py new file mode 100755 index 0000000000000000000000000000000000000000..fa045f65acb5feb4ac81ad2f5592ecc8f9c12054 --- /dev/null +++ b/src/assignment_9/src/exercise1_subscriber.py @@ -0,0 +1,52 @@ +#!/usr/bin/env python3 + +from cmath import pi +from math import sqrt + +import numpy as np +import rospy +import std_msgs.msg +from autominy_msgs.msg import NormalizedSteeringCommand, SpeedCommand +from nav_msgs.msg import Odometry +from tf.transformations import quaternion_from_euler, quaternion_multiply + + +class Subscriber: + def __init__(self): + rospy.init_node("movementPublisher") + self.vector = None + self.time = 0. + self.subscriber = rospy.Subscriber('/simulation/odom_ground_truth', Odometry, self.callback_pose) + self.steering_publisher = rospy.Publisher("/actuators/steering_normalized", NormalizedSteeringCommand, queue_size=10) + self.speed_publisher = rospy.Publisher("/actuators/speed", SpeedCommand, queue_size=10) + self.move(steering=0) + + def move(self, steering): + # Move approximately two meters + header = std_msgs.msg.Header() + rospy.loginfo("Start moving with steering angle %d", steering) + self.steering_publisher.publish(header, steering) + self.speed_publisher.publish(header, 0.3) + rospy.sleep(3) + rospy.loginfo("Stop moving") + header = std_msgs.msg.Header() + self.steering_publisher.publish(header, 0) + self.speed_publisher.publish(header, 0) + + + def callback_pose(self, message): + position = message.pose.pose.position + orientation = message.pose.pose.orientation + orientation = [orientation.x, orientation.y, orientation.z, orientation.w] + rospy.loginfo(self.__rotate_quaternion(orientation)) + + + @staticmethod + def __rotate_quaternion(quat): + # Rotate quat by 90 degree around x-axis. + q_rot = quaternion_from_euler(pi / 2, 0, 0) + return quaternion_multiply(q_rot, quat) + + +subscriber = Subscriber() +rospy.spin()