Merge branch 'develop'

Author: btalb

.gitignore

@@ -1,4 +1,2 @@
 *.sw*
 *.pyc
-
-debug*

nodes/debug_move_to_pose

@@ -0,0 +1,112 @@
+#!/usr/bin/env python
+
+from __future__ import print_function
+
+import numpy as np
+import os
+import rospy
+import tf2_ros
+import types
+
+from geometry_msgs.msg import Twist
+
+import sys
+
+sys.path.append(
+    os.path.join(os.path.dirname(os.path.realpath(__file__)), '../src'))
+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():
+
+        def __init__(self):
+            pass
+
+        def is_set(self):
+            return False
+
+    class DummyInstance():
+
+        def __init__(self):
+            pass
+
+        def is_collided(self):
+            return False
+
+        def is_dirty(self):
+            return False
+
+    def __init__(self):
+        self.evt = DummyController.DummyEvent()
+        self.instance = DummyController.DummyInstance()
+
+        self.config = {
+            'task': {
+                'name': 'ground_truth'
+            },
+            'robot': {
+                'global_frame': 'map',
+                'robot_frame': 'base_link'
+            }
+        }
+        self.state = {'initial_pose': 0}
+
+        self.tf_buffer = tf2_ros.Buffer()
+        self._tf_listener = tf2_ros.TransformListener(self.tf_buffer)
+
+
+if __name__ == '__main__':
+    rospy.init_node("debug_move_to_pose")
+
+    p = rospy.Publisher('/cmd_vel', Twist, queue_size=100)
+    d = DummyController()
+
+    # Wait for tf to exist (there's something really dodgy going on here...)
+    t = None
+    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:
+            pass
+
+    while not rospy.is_shutdown():
+        c = rc.__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(',')
+            if len(g) == 3:
+                goal_xyt = []
+                for x in g:
+                    try:
+                        goal_xyt.append(float(x))
+                    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_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)
+        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)

nodes/noisify_odom

@@ -0,0 +1,155 @@
+#!/usr/bin/env python
+#
+# This node takes a ground truth pose publishing system and converts it to the
+# noisy "odom -> base_link" + "map -> odom" corrections when localisation
+# approach typical in robotics.
+#
+# We assume that "map -> odom" is not currently defined, but is instead
+# provided by the "start_pose" param. The provided "odom -> base_link" is then
+# pulled from the odometry on "in_topic_odom".
+#
+# The "map -> base_link" transform is left unchanged, but it now consists of a
+# noisy "odom -> base_link" link plus ground truth "map -> odom" localisation
+# link.
+#
+# Noise model is basic zero-mean Gaussian noise on velocity, but scaled by the
+# velocity.  This leads to desirable results like 0 velocity having no noise /
+# drift, and is tolerable within our velocity limits. May look at this making
+# something more mature in the future. Current:
+#       Linear vel noise = N(0, linear_vel * noise_linear)
+#       Angular vel noise = N(0, angular_vel * noise_angular)
+
+import benchbot_spatial as sp
+import numpy as np
+import rospy
+import threading
+import tf2_ros
+import transforms3d as t3
+from geometry_msgs.msg import (Point, Pose, Quaternion, Transform,
+                               TransformStamped, Twist, Vector3)
+from nav_msgs.msg import Odometry
+from std_msgs.msg import String
+
+
+def _noisy(std_dev):
+    return std_dev * np.random.randn()
+
+
+def _SE3_to_pose_msg(pose):
+    return Pose(
+        position=Point(*pose[0:3, 3]),
+        orientation=Quaternion(
+            *np.roll(t3.quaternions.mat2quat(pose[0:3, 0:3]), -1)),
+    )
+
+
+def _SE3_to_tf_msg(pose):
+    return Transform(
+        translation=Vector3(*pose[0:3, 3]),
+        rotation=Quaternion(*np.roll(t3.quaternions.mat2quat(pose[0:3,
+                                                                  0:3]), -1)),
+    )
+
+
+class PoseNoisifier(object):
+
+    def __init__(self):
+        self.in_topic_odom = rospy.get_param("in_topic_odom", "/odom_raw")
+        self.in_topic_start_pose = rospy.get_param("in_topic_start_pose",
+                                                   "/odom_start_pose")
+        self.in_topic_vel = rospy.get_param("in_topic_vel", "/cmd_vel")
+
+        self.out_topic_odom = rospy.get_param("out_topic_odom", "/odom")
+        self.out_frame_map = rospy.get_param("out_frame_map", "map")
+        self.out_frame_odom = rospy.get_param("out_frame_odom", "odom")
+        self.out_frame_robot = rospy.get_param("out_frame_robot", "base_link")
+
+        self.noise_angular = rospy.get_param("noise_angular", 0.1)
+        self.noise_linear = rospy.get_param("noise_linear", 0.1)
+
+        self._lock = threading.Lock()
+
+        self._start_pose = None
+
+        self._accumulated_error = np.eye(4)
+        self._last_vel = Twist()
+
+        self._tf = tf2_ros.TransformBroadcaster()
+
+        self._pub_odom = rospy.Publisher(self.out_topic_odom,
+                                         Odometry,
+                                         queue_size=1)
+        self._sub_odom = rospy.Subscriber(self.in_topic_odom, Odometry,
+                                          self.pub)
+        self._sub_start_pose = rospy.Subscriber(self.in_topic_start_pose,
+                                                String, self.update_start_pose)
+        self._sub_vel = rospy.Subscriber(self.in_topic_vel, Twist,
+                                         self.update_vel)
+
+    def pub(self, odom_msg):
+        with self._lock:
+            # Bail if we don't have valid data to work with
+            if self._start_pose is None:
+                return
+
+            # Translate our clean odom into a noisy odom
+            noisy_odom = np.matmul(self._accumulated_error,
+                                   sp.pose_msg_to_SE3(odom_msg.pose))
+
+            # Figure out "localisation" transform from map to odom frame
+            localisation = np.matmul(self._start_pose,
+                                     np.linalg.inv(self._accumulated_error))
+
+            # Publish our messages
+            o = Odometry()
+            o.header.stamp = odom_msg.header.stamp
+            o.header.frame_id = self.out_frame_odom
+            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
+            self._pub_odom.publish(o)
+
+            t = TransformStamped()
+            t.header.stamp = odom_msg.header.stamp
+            t.header.frame_id = self.out_frame_map
+            t.child_frame_id = self.out_frame_odom
+            t.transform = _SE3_to_tf_msg(localisation)
+            self._tf.sendTransform(t)
+
+            t = TransformStamped()
+            t.header.stamp = odom_msg.header.stamp
+            t.header.frame_id = self.out_frame_odom
+            t.child_frame_id = self.out_frame_robot
+            t.transform = _SE3_to_tf_msg(noisy_odom)
+            self._tf.sendTransform(t)
+
+            # Update accumulated noise
+            self._accumulated_error = np.matmul(
+                self._accumulated_error,
+                np.matmul(
+                    sp.SE3_from_translation(self._last_vel.linear.x *
+                                            _noisy(self.noise_linear)),
+                    sp.SE3_from_yaw(self._last_vel.angular.z *
+                                    _noisy(self.noise_angular))),
+            )
+
+    def update_start_pose(self, start_pose_msg):
+        with self._lock:
+            self._accumulated_error = np.eye(4)
+            self._start_pose = sp.wxyzXYZ_to_SE3(*np.array([
+                float(x.strip()) for x in start_pose_msg.data.replace(
+                    '[', '').replace(']', '').split(',')
+            ]))
+
+    def update_vel(self, vel_msg):
+        with self._lock:
+            self._last_vel = vel_msg
+
+
+if __name__ == '__main__':
+    rospy.init_node("noisify_odom")
+
+    pn = PoseNoisifier()
+
+    rospy.spin()

scripts/pull_config

@@ -0,0 +1,5 @@
+#!/usr/bin/env bash
+
+# Pulls an existing config into currently running controller
+curl -s benchbot_supervisor:10000/config/ | \
+  curl -d @- -H 'Content-Type: application/json' benchbot_robot:10000/configure

setup.py

@@ -3,7 +3,8 @@
 from distutils.core import setup
 from catkin_pkg.python_setup import generate_distutils_setup
 
-d = generate_distutils_setup(packages=['benchbot_robot_controller'],
-                             package_dir={'': 'src'})
+d = generate_distutils_setup(
+    packages=['benchbot_robot_controller', 'benchbot_spatial'],
+    package_dir={'': 'src'})
 
 setup(**d)