assignment_9 exercise1

src/assignment_9/src/exercise1_subscriber.py

+#!/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()