Remember to import all the message types which will be further used, along with rospy
import rospy
import math
from sensor_msgs.msg import Imu
from geometry_msgs.msg import Point, Quaternion
from mavros_msgs.msg import OverrideRCIn
self.imu_subscriber = rospy.Subscriber("/mavros/imu/data", Imu, self.imu_callback)
The above command is used to subscribe to the rostopic ‘*mavros/imu/data’ when the function ‘*self.imu_callback’ is called. The message type is ‘Imu’ which consists of orientation, angular_velocity and linear_acceleration.
To access orientation data from imu, the following method is implemented in the imu_callback function (self.orientation is a variable created for the class)
def imu_callback(self, data):
self.orientation.x = data.orientation.x
self.orientation.y = data.orientation.y
self.orientation.z = data.orientation.z
self.orientation.w = data.orientation.w
The following is used to get euler angles to find the counnter movement of manipulator
def euler_from_quaternion(self):
x = self.orientation.x
y = self.orientation.y
z = self.orientation.z
w = self.orientation.w
# Roll (x-axis rotation)
t0 = +2.0 * (w * x + y * z)
t1 = +1.0 - 2.0 * (x * x + y * y)
self.roll = math.atan2(t0, t1)
# Pitch (y-axis rotation)
t2 = +2.0 * (w * y - z * x)
t2 = +1.0 if t2 > +1.0 else t2
t2 = -1.0 if t2 < -1.0 else t2
self.pitch = math.asin(t2)
# Yaw (z-axis rotation)
t3 = +2.0 * (w * z + x * y)
t4 = +1.0 - 2.0 * (y * y + z * z)
self.yaw = math.atan2(t3, t4)
self.roll = math.degrees(self.roll)
self.pitch = math.degrees(self.pitch)
self.yaw = math.degrees(self.yaw)
The counter-movement of the drone pitch is implemented on servo to keep the arm vertical. The PWM input for servo varies from 1000 to 2000. This is currently given by the radio transmitter inputs (in this case Channel 7 controls bigger arm and Channel 8 controls smaller arm). It was found that the servo approximately moves as described: