Merge branch 'develop'

Author: btalb

LICENSE.txt

@@ -1,4 +1,4 @@
-Copyright (c) 2020, Queensland University of Technology (Ben Talbot, David Hall, Haoyang Zhang, Suman Bista, Rohan Smith, Niko Sünderhauf, and Feras Dayoub)
+Copyright (c) 2020, Queensland University of Technology (Ben Talbot, David Hall, and Niko Sünderhauf)
 All rights reserved.
 
 Redistribution and use in source and binary forms, with or without modification,

nodes/debug_move_to_pose

@@ -2,6 +2,7 @@
 
 from __future__ import print_function
 
+import benchbot_spatial as sp
 import numpy as np
 import os
 import rospy
@@ -17,11 +18,6 @@ sys.path.append(
 import robot_callbacks as rc
 
 
-def __SE2_to_SE3(pose):
-    return np.matmul(rc.__SE3_from_translation(pose[0, 2], pose[1, 2]),
-                     rc.__SE3_from_yaw(rc.__yaw_from_SE2(pose)))
-
-
 class DummyController():
 
     class DummyEvent():
@@ -70,22 +66,28 @@ if __name__ == '__main__':
 
     # Wait for tf to exist (there's something really dodgy going on here...)
     t = None
+    print("Waiting for tf...")
     while not rospy.is_shutdown() and t is None:
         try:
             t = d.tf_buffer.lookup_transform('map', 'base_link',
                                              rospy.Time.now(),
                                              rospy.Duration(1.0))
         except Exception as e:
+            print("ERROR: %s" % e)
             pass
+    print("Got initial tf.")
 
     while not rospy.is_shutdown():
-        c = rc.__SE3_to_SE2(rc._current_pose(d))
+        c = sp.SE3_to_SE2(rc._current_pose(d))
 
         # Get goal from terminal
         goal_xyt = None
         while goal_xyt is None:
             print("Relative goal (format: x,y,th): ", end='')
-            g = raw_input().split(',')
+            r = raw_input()
+            if len(r) == 0:
+                sys.exit(0)
+            g = r.split(',')
             if len(g) == 3:
                 goal_xyt = []
                 for x in g:
@@ -94,19 +96,19 @@ if __name__ == '__main__':
                     except:
                         goal_xyt = None
                         break
-        goal_rel = rc.__SE3_to_SE2(
-            np.matmul(rc.__SE3_from_translation(goal_xyt[0], goal_xyt[1]),
-                      rc.__SE3_from_yaw(goal_xyt[2])))
+        goal_rel = sp.SE3_to_SE2(
+            np.matmul(sp.SE3_from_translation(goal_xyt[0], goal_xyt[1]),
+                      sp.SE3_from_yaw(goal_xyt[2])))
         goal_abs = np.matmul(c, goal_rel)
 
         # Print some information about the goal
-        ct = rc.__SE2_to_xyt(c)
-        grt = rc.__SE2_to_xyt(goal_rel)
-        gat = rc.__SE2_to_xyt(goal_abs)
+        ct = sp.SE2_to_xyt(c)
+        grt = sp.SE2_to_xyt(goal_rel)
+        gat = sp.SE2_to_xyt(goal_abs)
         print(
             "Going from:\t(%f,%f,%f)\n\tto\t(%f,%f,%f)\n\tdelta\t(%f,%f,%f)" %
             (ct[0], ct[1], ct[2], gat[0], gat[1], gat[2], grt[0], grt[1],
              grt[2]))
 
         # Try and move to the goal
-        rc._move_to_pose(__SE2_to_SE3(goal_abs), p, d)
+        rc._move_to_pose(sp.SE2_to_SE3(goal_abs), p, d)

nodes/noisify_odom

@@ -107,7 +107,7 @@ class PoseNoisifier(object):
             o.child_frame_id = self.out_frame_robot
             o.pose.pose = _SE3_to_pose_msg(noisy_odom)
             o.pose.covariance = odom_msg.pose.covariance
-            o.twist = self._last_vel
+            o.twist.twist = self._last_vel
             self._pub_odom.publish(o)
 
             t = TransformStamped()

package.xml

@@ -1,7 +1,7 @@
 <?xml version="1.0"?>
 <package format="2">
   <name>benchbot_robot_controller</name>
-  <version>2.2.1</version>
+  <version>2.4.0</version>
   <description>The benchbot_robot_controller package</description>
 
   <maintainer email="b.talbot@qut.edu.au">Ben Talbot</maintainer>

src/benchbot_spatial.py

@@ -29,6 +29,11 @@ def rpy_from_SE3(pose):
     return t3.euler.mat2euler(pose[0:3, 0:3])
 
 
+def SE2_to_SE3(pose):
+    return np.matmul(SE3_from_translation(pose[0, 2], pose[1, 2]),
+                     SE3_from_yaw(yaw_from_SE2(pose)))
+
+
 def SE2_to_xyt(pose):
     return np.array([pose[0, 2], pose[1, 2], yaw_from_SE2(pose)])
 

src/robot_callbacks.py

@@ -3,6 +3,7 @@
 import ros_numpy
 import rospy
 from geometry_msgs.msg import Twist
+from time import time
 
 _DEFAULT_SPEED_FACTOR = 1
 
@@ -13,12 +14,14 @@
 
 _MOVE_ANGLE_K = 3
 
-_MOVE_POSE_K_RHO = 1
-_MOVE_POSE_K_ALPHA = 4
-_MOVE_POSE_K_BETA = -1.5
+_MOVE_TIMEOUT = 30
 
-_MOVE_LINEAR_LIMITS = [-0.6, 2.0]
-_MOVE_ANGULAR_LIMIT = 0.5
+_MOVE_POSE_K_RHO = 2
+_MOVE_POSE_K_ALPHA = 10
+_MOVE_POSE_K_BETA = -4
+
+_MOVE_LINEAR_LIMITS = [-0.6, 1.5]
+_MOVE_ANGULAR_LIMIT = 1.0
 
 
 def __safe_dict_get(d, key, default):
@@ -79,7 +82,8 @@ def _move_to_angle(goal, publisher, controller):
     gamma = None
     vel_msg = Twist()
     hz_rate = rospy.Rate(_MOVE_HZ)
-    while (not controller.evt.is_set() and
+    t = time()
+    while (time() - t < _MOVE_TIMEOUT and not controller.evt.is_set() and
            not controller.instance.is_collided() and
            (gamma is None or np.abs(gamma) > _MOVE_TOL_YAW)):
         # Get latest orientation error
@@ -106,9 +110,11 @@ def _move_to_pose(goal, publisher, controller):
     g = sp.SE3_to_SE2(goal)
 
     rho = None
+    backwards = None
     vel_msg = Twist()
     hz_rate = rospy.Rate(_MOVE_HZ)
-    while (not controller.evt.is_set() and
+    t = time()
+    while (time() - t < _MOVE_TIMEOUT and not controller.evt.is_set() and
            not controller.instance.is_collided() and
            (rho is None or rho > _MOVE_TOL_DIST)):
         # Get latest position error
@@ -118,11 +124,13 @@ def _move_to_pose(goal, publisher, controller):
         # Calculate values used in the controller
         rho = np.linalg.norm(error[0:2, 2])
         alpha = np.arctan2(error[1, 2], error[0, 2])
-        beta = sp.pi_wrap(-sp.yaw_from_SE2(current) - alpha + sp.yaw_from_SE2(g))
+        beta = sp.pi_wrap(-sp.yaw_from_SE2(current) - alpha +
+                          sp.yaw_from_SE2(g))
 
         # Construct velocity message
-        backwards = (rho > _MOVE_TOL_DIST and
-                     np.abs(np.arctan2(error[1, 2], error[0, 2])) > np.pi / 2)
+        if backwards is None:
+            backwards = (np.abs(np.arctan2(error[1, 2], error[0, 2])) >
+                         1.05 * np.pi / 2)
         if backwards:
             vel_msg.linear.x = -1 * _MOVE_POSE_K_RHO * rho
             vel_msg.angular.z = (
@@ -137,20 +145,25 @@ def _move_to_pose(goal, publisher, controller):
         vel_msg.linear.x = _move_speed_factor(controller) * vel_msg.linear.x
         vel_msg.angular.z = _move_speed_factor(controller) * vel_msg.angular.z
 
-        vel_msg.linear.x = (
-            _MOVE_LINEAR_LIMITS[1] if vel_msg.linear.x > _MOVE_LINEAR_LIMITS[1]
-            else _MOVE_LINEAR_LIMITS[0]
-            if vel_msg.linear.x < _MOVE_LINEAR_LIMITS[0] else vel_msg.linear.x)
-        vel_msg.angular.z = (
-            _MOVE_ANGULAR_LIMIT if vel_msg.angular.z > _MOVE_ANGULAR_LIMIT else
-            -_MOVE_ANGULAR_LIMIT
-            if vel_msg.angular.z < -_MOVE_ANGULAR_LIMIT else vel_msg.angular.z)
+        clamped_l = min(_MOVE_LINEAR_LIMITS[1],
+                        max(_MOVE_LINEAR_LIMITS[0], vel_msg.linear.x))
+        clamped_a = min(_MOVE_ANGULAR_LIMIT,
+                        max(-_MOVE_ANGULAR_LIMIT, vel_msg.angular.z))
+        factor = max(abs(vel_msg.linear.x / clamped_l),
+                     abs(vel_msg.angular.z / clamped_a))
+        # print(
+        #     "r,a,p: (%f,%f,%f) B:%d l,a: (%f, %f) lc,ac: (%f, %f)" %
+        #     (rho, alpha, beta, backwards, vel_msg.linear.x, vel_msg.angular.z,
+        #      vel_msg.linear.x / factor, vel_msg.angular.z / factor))
+        vel_msg.linear.x = vel_msg.linear.x / factor
+        vel_msg.angular.z = vel_msg.angular.z / factor
 
         # Publish velocity (don't move if we're already there!)
         if (rho > _MOVE_TOL_DIST):
             publisher.publish(vel_msg)
             hz_rate.sleep()
-    _move_to_angle(goal, publisher, controller)
+    if (time() - t < _MOVE_TIMEOUT):
+        _move_to_angle(goal, publisher, controller)
     publisher.publish(Twist())
 
 
@@ -240,10 +253,10 @@ def move_angle(data, publisher, controller):
 def move_distance(data, publisher, controller):
     # Derive a corresponding goal pose & send the robot there
     _move_to_pose(
-        np.matmul(_current_pose(controller),
-                  sp.SE3_from_translation(__safe_dict_get(data, 'distance',
-                                                         0))), publisher,
-        controller)
+        np.matmul(
+            _current_pose(controller),
+            sp.SE3_from_translation(__safe_dict_get(data, 'distance', 0))),
+        publisher, controller)
 
 
 def move_next(data, publisher, controller):