Localization error monitor created. Spiral trajectory cmd_vel script …

…created. (#92)

* Fiducials world created

* Paper background added to markers

* Localization monitor created. cmd_vel trajectory publisher created.

* Plot update rate configured. Refactoring.

* Localization data errors print added.

magni_description/urdf/magni.gazebo.xacro

@@ -9,6 +9,7 @@
 
     <gazebo>
         <plugin name="gazebo_ros_p3d" filename="libgazebo_ros_p3d.so">
+            <updateRate>100</updateRate>
             <bodyName>base_footprint</bodyName>
             <topicName>exact_pose</topicName>
             <frameName>map</frameName>

magni_gazebo/launch/fiducials/fiducial_slam_gazebo.launch

@@ -0,0 +1,20 @@
+<launch>
+
+    <include file="$(find aruco_detect)/launch/aruco_detect.launch">
+        <arg name="camera" value="/raspicam_node"/>
+        <arg name="image" value="image/compressed"/>
+        <arg name="transport" default="compressed"/>
+        <arg name="fiducial_len" value="0.14"/>
+        <arg name="dictionary" value="16"/>
+    </include>
+
+    <include file="$(find fiducial_slam)/launch/fiducial_slam.launch">
+        <arg name="camera" value="/raspicam_node"/>
+        <arg name="map_frame" value="map"/>
+        <arg name="odom_frame" value="odom"/>
+        <arg name="base_frame" value="base_footprint"/>
+        <arg name="future_date_transforms" value="0.5"/>
+        <arg name="publish_6dof_pose" value="true"/>
+    </include>
+
+</launch>

magni_gazebo/launch/fiducials/fiducial_world.launch

@@ -5,17 +5,10 @@
         <arg name="sonars_installed" value="false"/>
     </include>
 
+    <!-- spawn feducial markers -->
     <include file="$(find magni_gazebo)/launch/fiducials/fiducial_grid_spawn.launch"/>
 
-
-    <include file="$(find aruco_detect)/launch/aruco_detect.launch">
-        <arg name="camera" value="/raspicam_node"/>
-        <arg name="image" default="image/compressed"/>
-        <arg name="dictionary" value="16"/>
-    </include>
-
-    <include file="$(find fiducial_slam)/launch/fiducial_slam.launch">
-        <arg name="camera" value="/raspicam_node"/>
-    </include>
+    <!-- run feducial SLAM -->
+    <include file="$(find magni_gazebo)/launch/fiducials/fiducial_slam_gazebo.launch"/>
 
 </launch>

magni_nav/scripts/localization_monitor/localization_monitor.py

@@ -0,0 +1,235 @@
+#!/usr/bin/env python
+import rospy
+from nav_msgs.msg import Odometry
+from  geometry_msgs.msg import Pose, Point, Quaternion
+import tf.transformations
+import math
+import threading
+import numpy as np
+import matplotlib.pyplot as plt
+
+
+def odom_callback(msg):
+    lock.acquire()
+
+    global exact_pose
+    global is_exact_init
+    exact_pose = msg.pose.pose
+    is_exact_init = True
+
+    lock.release()
+
+
+def update_monitor():
+
+    global is_exact_init
+
+    if not is_exact_init:
+        return
+
+    # get SLAM estimated pose from transform
+    try:
+        tf_listener.waitForTransform('map', 'base_footprint', rospy.Time(0), rospy.Duration(3))
+        (slam_trans, slam_rot) = tf_listener.lookupTransform('map', 'base_footprint', rospy.Time(0))
+    except (tf.LookupException, tf.ConnectivityException, tf.ExtrapolationException):
+        return
+
+    slam_pose = Pose()
+    slam_pose.position.x = slam_trans[0]
+    slam_pose.position.y = slam_trans[1]
+    slam_pose.position.z = slam_trans[2]
+    slam_pose.orientation.x = slam_rot[0]
+    slam_pose.orientation.y = slam_rot[1]
+    slam_pose.orientation.z = slam_rot[2]
+    slam_pose.orientation.w = slam_rot[3]
+    slam_roll,slam_pitch,slam_yaw = tf.transformations.euler_from_quaternion(slam_rot)
+    rospy.loginfo('\n---------------------------------------------')
+    rospy.loginfo('\nSLAM pose: ' + str(slam_pose))
+
+    # global vars
+    lock.acquire()
+
+    global exact_pose
+    global samples
+    global error_accum_x
+    global error_accum_y
+    global error_accum_z
+    global error_accum_magnitude
+    global error_accum_yaw
+    global data_time
+    global data_error_x
+    global data_error_y
+    global data_error_z
+    global data_error_magnitude
+    global data_error_roll
+    global data_error_pitch
+    global data_error_yaw
+    global data_dev_x
+    global data_dev_y
+    global data_dev_z
+    global data_dev_magnitude
+    global data_dev_yaw
+    global next_plot_time
+
+    # get exact pose from exact Odometry topic
+    exact_roll,exact_pitch,exact_yaw = tf.transformations.euler_from_quaternion(
+        [exact_pose.orientation.x, exact_pose.orientation.y, exact_pose.orientation.z, exact_pose.orientation.w])
+    rospy.loginfo('\nExact pose: ' + str(exact_pose))
+
+    # calculate errors
+    error_x = slam_pose.position.x - exact_pose.position.x
+    error_y = slam_pose.position.y - exact_pose.position.y
+    error_z = slam_pose.position.z - exact_pose.position.z
+    error_magnitude = math.sqrt(error_x*error_x + error_y*error_y + error_z*error_z)
+    error_roll = slam_roll - exact_roll
+    error_pitch = slam_pitch - exact_pitch
+    error_yaw = slam_yaw - exact_yaw
+
+    # calculate standard deviations
+    samples += 1
+    error_accum_x += error_x * error_x
+    error_accum_y += error_y * error_y
+    error_accum_z += error_z * error_z
+    error_accum_magnitude += error_magnitude * error_magnitude
+    error_accum_yaw += error_yaw * error_yaw
+
+    sdev_x = math.sqrt(error_accum_x / samples)
+    sdev_y = math.sqrt(error_accum_y / samples)
+    sdev_z = math.sqrt(error_accum_z / samples)
+    sdev_magnitude = math.sqrt(error_accum_magnitude / samples)
+    sdev_yaw = math.sqrt(error_accum_yaw / samples)
+
+    lock.release()
+
+    # errors data
+    data_time = np.append(data_time, rospy.Time.now().to_sec())
+    data_error_x = np.append(data_error_x, error_x)
+    data_error_y = np.append(data_error_y, error_y)
+    data_error_z = np.append(data_error_z, error_z)
+    data_error_magnitude = np.append(data_error_magnitude, error_magnitude)
+    # angle errors
+    data_error_roll = np.append(data_error_roll, error_roll)
+    data_error_pitch = np.append(data_error_pitch, error_pitch)
+    data_error_yaw = np.append(data_error_yaw, error_yaw)
+    # standard deviations
+    data_dev_x = np.append(data_dev_x, sdev_x)
+    data_dev_y = np.append(data_dev_y, sdev_y)
+    data_dev_z = np.append(data_dev_z, sdev_z)
+    data_dev_magnitude = np.append(data_dev_magnitude, sdev_magnitude)
+    data_dev_yaw = np.append(data_dev_yaw, sdev_yaw)
+
+    if samples == 1 or rospy.Time.now() >= next_plot_time:
+        next_plot_time = rospy.Time.now() + rospy.Duration(1)
+        # print results
+        printErrorData()
+        # draw plots
+        error_pos_plot.clear()
+        error_pos_plot.title.set_text("Positin errors: X, Y, Z, Magnitude")
+        error_pos_plot.plot(data_time, data_error_x, 'r-')
+        error_pos_plot.plot(data_time, data_error_y, 'g-')
+        error_pos_plot.plot(data_time, data_error_z, 'b-')
+        error_pos_plot.plot(data_time, data_error_magnitude, 'm-')
+        error_angle_plot.clear()
+        error_angle_plot.title.set_text("Rotation errors: Roll, Pitch, Yaw")
+        error_angle_plot.plot(data_time, data_error_roll, 'r-')
+        error_angle_plot.plot(data_time, data_error_pitch, 'g-')
+        error_angle_plot.plot(data_time, data_error_yaw, 'b-')
+        deviation_plot.clear()
+        deviation_plot.title.set_text("Standard deviations: X, Y, Z, Magnitude")
+        deviation_plot.plot(data_time, data_dev_x, 'r-')
+        deviation_plot.plot(data_time, data_dev_y, 'g-')
+        deviation_plot.plot(data_time, data_dev_z, 'b-')
+        deviation_plot.plot(data_time, data_dev_magnitude, 'm-')
+        # histograms
+        pos_hist.clear()
+        pos_hist.title.set_text("Position error: X")
+        pos_hist.hist(data_error_x, bins='auto')
+        pos_magnitude_hist.clear()
+        pos_magnitude_hist.title.set_text("Position error: Magnitude")
+        pos_magnitude_hist.hist(data_error_magnitude, bins='auto')
+
+        fig.canvas.draw()
+        fig.canvas.flush_events()
+
+
+def printErrorData():
+    rospy.loginfo("\n====================\nLocalization data:" +
+            "\nPosition errors - Magnitude: " + str(data_error_magnitude[-1]) + ", X: " + str(data_error_x[-1]) +
+                  ", Y: " + str(data_error_y[-1]) + ", Z: " + str(data_error_z[-1]) +
+            "\nRotation errors - Roll: " + str(data_error_roll[-1]) + ", Pitch: " + str(data_error_pitch[-1]) +
+                  ", Yaw: " + str(data_error_yaw[-1]) +
+            "\nStandard deviations - Magnitude: " + str(data_dev_magnitude[-1]) + ", X: " + str(data_dev_x[-1]) +
+                  ", Y: " + str(data_dev_y[-1]) + ", Z: " + str(data_dev_z[-1]) +
+            "\n====================")
+
+
+if __name__ == '__main__':
+
+    global exact_pose
+    global is_exact_init
+    global samples
+    global error_accum_x
+    global error_accum_y
+    global error_accum_z
+    global error_accum_magnitude
+    global error_accum_yaw
+    global data_time
+    global data_error_x
+    global data_error_y
+    global data_error_z
+    global data_error_magnitude
+    global data_error_roll
+    global data_error_pitch
+    global data_error_yaw
+    global data_dev_x
+    global data_dev_y
+    global data_dev_z
+    global data_dev_magnitude
+    global data_dev_yaw
+
+    lock = threading.Lock()
+    exact_pose = Odometry()
+    is_exact_init = False
+
+    samples = 0
+    error_accum_x = 0.0
+    error_accum_y = 0.0
+    error_accum_z = 0.0
+    error_accum_magnitude = 0.0
+    error_accum_yaw = 0.0
+
+    data_time = np.array([])
+    data_error_x = np.array([])
+    data_error_y = np.array([])
+    data_error_z = np.array([])
+    data_error_magnitude = np.array([])
+    data_error_roll = np.array([])
+    data_error_pitch = np.array([])
+    data_error_yaw = np.array([])
+    data_dev_x = np.array([])
+    data_dev_y = np.array([])
+    data_dev_z = np.array([])
+    data_dev_magnitude = np.array([])
+    data_dev_yaw = np.array([])
+
+    rospy.init_node('localization_monitor', anonymous=True)
+    tf_listener = tf.TransformListener()
+    rospy.Subscriber('/exact_pose', Odometry, odom_callback)
+
+    plt.ion()
+    fig = plt.figure()
+    fig.canvas.set_window_title('Localization monitor')
+    error_pos_plot = fig.add_subplot(321)
+    error_angle_plot = fig.add_subplot(323)
+    deviation_plot = fig.add_subplot(325)
+    pos_hist = fig.add_subplot(322)
+    pos_magnitude_hist = fig.add_subplot(324)
+    fig.show()
+
+    # print errors data on exit
+    rospy.on_shutdown(printErrorData)
+    # run update loop
+    r = rospy.Rate(5)  # 5hz
+    while not rospy.is_shutdown():
+        update_monitor()
+        r.sleep()

magni_nav/scripts/localization_monitor/spiral_cmd.py

@@ -0,0 +1,92 @@
+#!/usr/bin/env python
+import rospy
+from geometry_msgs.msg import Twist
+import math
+
+step_size = 1.0
+spiral_diam = 7
+
+# linear speed params
+a_x = 1.17
+v_x = 0.2
+a_x_t = v_x/a_x
+a_dist = a_x * a_x_t**2 / 2
+# angular speed params
+a_z = 2.02
+v_z = 0.45
+a_z_t = v_z/a_z
+a_rad = a_z * a_z_t**2 / 2
+
+twist = Twist()
+
+
+def travel_time(dist):
+    t = (dist - a_dist) / v_x + a_x_t
+    return t
+
+
+def rotation_time(rad):
+    t = (rad - a_rad) / v_z + a_z_t
+    return t
+
+
+def publish_cmd_vel(twist, t):
+    rospy.loginfo("publish cmd:\n" + str(twist))
+    end_time = rospy.rostime.get_rostime() + rospy.Duration(t)
+    rate = rospy.Rate(100)
+    while True:
+        pub.publish(twist)
+        rate.sleep()
+        rem_time = end_time - rospy.rostime.get_rostime()
+        if rem_time < rate.sleep_dur:
+            if rem_time.to_sec() > 0:
+                rospy.sleep(rem_time)
+            break
+
+
+def move(dist):
+    twist.linear.x = v_x
+    twist.angular.z = 0
+    t = travel_time(dist)
+    rospy.loginfo("move " + str(dist) + " m for " + str(t) + " sec")
+    publish_cmd_vel(twist, t)
+
+
+def rotate(rad):
+    twist.linear.x = 0
+    twist.angular.z = v_z
+    t = rotation_time(rad)
+    rospy.loginfo("rotate " + str(rad) + " rad for " + str(t) + " sec")
+    publish_cmd_vel(twist, t)
+
+
+def stop():
+    twist.linear.x = 0
+    twist.angular.z = 0
+    rospy.loginfo("stop");
+    publish_cmd_vel(twist, 0.3)
+
+
+if __name__ == '__main__':
+
+    pub = rospy.Publisher('cmd_vel', Twist, queue_size = 1)
+    rospy.init_node('spiral_cmd', anonymous=True)
+
+    try:
+        for i in range(1, spiral_diam):
+            dist = i * step_size
+            if i < spiral_diam:
+                move(dist)
+                stop()
+                rotate(math.pi/2)
+                stop()
+                move(dist)
+                stop()
+                rotate(math.pi/2)
+                stop()
+            else:
+                move(dist)
+                stop()
+    except rospy.ROSInterruptException:
+        pass
+