Merge pull request #95 from hello-robot/feature/autodocking

Autodocking with Nav stack demo

stretch_core/config/stretch_marker_dict.yaml

@@ -37,6 +37,11 @@
     'use_rgb_only': False
     'name': 'shoulder_top'
     'link': 'link_aruco_shoulder'
+  '245':
+    'length_mm': 88.0
+    'use_rgb_only': False
+    'name': 'docking_station'
+    'link': None
   '246':
     'length_mm': 179.0
     'use_rgb_only': False

stretch_demos/CMakeLists.txt

@@ -8,6 +8,7 @@ project(stretch_demos)
 ## if COMPONENTS list like find_package(catkin REQUIRED COMPONENTS xyz)
 ## is used, also find other catkin packages
 find_package(catkin REQUIRED COMPONENTS
+  genmsg
   actionlib
   actionlib_msgs
   geometry_msgs
@@ -28,7 +29,7 @@ find_package(catkin REQUIRED COMPONENTS
 ## Uncomment this if the package has a setup.py. This macro ensures
 ## modules and global scripts declared therein get installed
 ## See http://ros.org/doc/api/catkin/html/user_guide/setup_dot_py.html
-# catkin_python_setup()
+catkin_python_setup()
 
 ################################################
 ## Declare ROS messages, services and actions ##
@@ -69,17 +70,17 @@ find_package(catkin REQUIRED COMPONENTS
 # )
 
 ## Generate actions in the 'action' folder
-# add_action_files(
-#   FILES
-#   Action1.action
-#   Action2.action
-# )
+add_action_files(
+  FILES
+  ArucoHeadScan.action
+  VisualServo.action
+)
 
 ## Generate added messages and services with any dependencies listed here
-# generate_messages(
-#   DEPENDENCIES
-#   actionlib_msgs#   geometry_msgs#   nav_msgs#   std_msgs
-# )
+generate_messages(
+  DEPENDENCIES
+  actionlib_msgs
+)
 
 ################################################
 ## Declare ROS dynamic reconfigure parameters ##

stretch_demos/README.md

@@ -54,6 +54,16 @@ roslaunch stretch_demos clean_surface.launch
 
 Then, press the key with the / and ? on it while in the terminal to initiate a surface cleaning attempt.
 
+### Autodocking with Nav Stack
+
+For this demo, the robot will look for a docking station with an ArUco marker, align itself to the docking station using the ROS Nav Stack, and then back up into the docking station using an error-based controller. Prior to running this demo, you should stow the robot, ensure that you have a pregenerated map and the docking station is resting against a wall. Go through [this]() tutorial to understand how the demo works.
+
+You can launch this demo using the following command:
+
+```
+roslaunch stretch_demos autodocking.launch
+```
+
 ## License
 
 For license information, please see the LICENSE files. 

stretch_demos/action/ArucoHeadScan.action

@@ -0,0 +1,28 @@
+# Define the goal
+
+# Specify aruco ID to look for
+uint32 aruco_id
+
+# Specify the camera tilt_angle at which to scan
+float32 tilt_angle
+
+# Publish tf relative to the map frame
+bool publish_to_map
+
+# If robot should rotate to cover the blind spot generated by the mast
+bool fill_in_blindspot_with_second_scan 
+
+# If scan should stop as soon as a matching aruco ID is found
+bool fast_scan
+
+---
+# Define the result
+
+# Whether goal aruco ID was found
+bool aruco_found
+
+---
+# Define a feedback message
+
+# Pan angle of the camera
+float32 pan_angle

stretch_demos/action/VisualServo.action

@@ -0,0 +1,19 @@
+# goal
+# target frame defines the transform frame that needs to be compared with source_frame
+string target_frame
+
+# target frame defines the transform frame that needs to be compared with target_frame
+string source_frame
+
+---
+# result
+# result is the success flag
+bool result
+
+---
+# feedback
+# angle error is the angular error between source and target frames
+float64 angle_error
+
+# distance error is the euclidean error between source and target frames
+float64 distance_error

stretch_demos/launch/autodocking.launch

@@ -0,0 +1,65 @@
+<launch>
+
+    <arg name="rviz"            default="true"                                     doc="whether to show Rviz" />
+    <arg name="map_yaml"        default="$(env HELLO_FLEET_PATH)/maps/xyz.yaml"    doc="previously captured map (optional)" />
+    <arg name="debug_directory" default="$(env HELLO_FLEET_PATH)/debug/"           doc="directory where debug imagery is saved" />
+    <arg name="trees_viz"       default="true"                                     doc="whether to visualize BT in rqt" />
+
+    <!-- REALSENSE D435i -->
+    <include file="$(find stretch_core)/launch/d435i_high_resolution.launch"></include>
+    <node name="d435i_configure" pkg="stretch_core" type="d435i_configure" output="screen">
+        <!--<param name="initial_mode" type="string" value="Default"/>-->
+        <param name="initial_mode" type="string" value="High Accuracy"/>
+    </node>
+    <!-- -->
+
+    <!-- FRUSTUM FIELD OF VIEW VISUALIZATION -->
+    <node name="d435i_frustum_visualizer" pkg="stretch_core" type="d435i_frustum_visualizer" output="screen" />
+    <!-- -->
+
+    <!-- STRETCH DRIVER -->
+    <param name="/stretch_driver/broadcast_odom_tf" type="bool" value="true"/>
+    <param name="/stretch_driver/mode" type="string" value="navigation" />
+    <include file="$(find stretch_core)/launch/stretch_driver.launch" pass_all_args="true"/>
+    <!-- -->
+
+    <!-- CENTERED BASE LINK -->
+    <node name="centered_base_link_tf_publisher" pkg="tf" type="static_transform_publisher" args="-0.1 0 0 0 0 0 1 /base_link /centered_base_link 100" />
+
+    <!-- LASER RANGE FINDER -->
+    <include file="$(find stretch_core)/launch/rplidar.launch" />
+
+    <!-- MAP SERVER -->
+    <node name="map_server" pkg="map_server" type="map_server" args="$(arg map_yaml)" />
+
+    <!-- LOCALIZATION -->
+    <include file="$(find amcl)/examples/amcl_diff.launch" />
+
+    <!-- ARUCO DETECTION -->
+    <include file="$(find stretch_core)/launch/stretch_aruco.launch" />
+
+    <!-- NAVIGATION -->
+    <node pkg="move_base" type="move_base" respawn="false" name="move_base" output="screen">
+        <rosparam file="$(find stretch_navigation)/config/common_costmap_params.yaml" command="load" ns="global_costmap" />
+        <rosparam file="$(find stretch_navigation)/config/common_costmap_params.yaml" command="load" ns="local_costmap" />
+        <rosparam file="$(find stretch_navigation)/config/local_costmap_params.yaml" command="load" />
+        <rosparam file="$(find stretch_navigation)/config/global_costmap_params_withmap.yaml" command="load" />
+        <rosparam file="$(find stretch_navigation)/config/base_local_planner_params.yaml" command="load" />
+
+        <remap from="/cmd_vel" to="/stretch/cmd_vel" />
+    </node>
+
+    <!-- VISUALIZE -->
+    <node name="rviz" pkg="rviz" type="rviz" output="log" args="-d $(find stretch_navigation)/rviz/navigation.rviz" if="$(arg rviz)" />
+    <node pkg="rqt_py_trees" name="rqt_py_trees" type="rqt_py_trees" if="$(arg trees_viz)"/>
+    
+    <!-- ARUCO ACTION SERVER -->
+    <node name="aruco_head_scan" pkg="stretch_demos" type="aruco_head_scan_action.py" output="screen" />
+
+    <!-- VISUAL SERVOING ACTION SERVER -->
+    <node name="visual_servoing" pkg="stretch_demos" type="visual_servoing_action.py" output="screen" />
+
+    <!-- AUTODOCKING -->
+    <node name="autodocking" pkg="stretch_demos" type="autodocking_bt.py" output="screen" />
+
+</launch>

stretch_demos/nodes/aruco_head_scan_action.py

@@ -0,0 +1,205 @@
+#!/usr/bin/env python3
+
+import rospy
+from visualization_msgs.msg import MarkerArray
+from geometry_msgs.msg import Transform, TransformStamped, Pose
+import ros_numpy
+import numpy as np
+import tf2_ros
+import time
+
+import actionlib
+from stretch_demos.msg import ArucoHeadScanAction, ArucoHeadScanGoal, ArucoHeadScanFeedback, ArucoHeadScanResult
+from trajectory_msgs.msg import JointTrajectoryPoint
+from control_msgs.msg import FollowJointTrajectoryGoal
+from tf.transformations import euler_from_quaternion, quaternion_from_euler
+
+from sensor_msgs.msg import JointState
+from sensor_msgs.msg import PointCloud2
+import hello_helpers.hello_misc as hm
+
+class ArucoHeadScan(hm.HelloNode):
+    def __init__(self):
+        hm.HelloNode.__init__(self)
+        hm.HelloNode.main(self, 'aruco_head_scan', 'aruco_head_scan', wait_for_first_pointcloud=False)
+        self.server = actionlib.SimpleActionServer('ArucoHeadScan', ArucoHeadScanAction, self.execute_cb, False)
+        self.goal = ArucoHeadScanGoal()
+        self.feedback = ArucoHeadScanFeedback()
+        self.result = ArucoHeadScanResult()
+        self.aruco_marker_array = rospy.Subscriber('aruco/marker_array', MarkerArray, self.aruco_callback)
+        self.predock_pose_pub = rospy.Publisher('/predock_pose', Pose, queue_size=10)
+        self.dock_pose_pub = rospy.Publisher('/dock_pose', Pose, queue_size=10)
+        self.server.start()
+        self.aruco_id = 1000 # Placeholder value, can never be true
+        self.aruco_found = False
+        self.markers = MarkerArray().markers
+
+    def execute_cb(self, goal):
+        self.goal = goal
+        self.aruco_id = self.goal.aruco_id
+        self.tilt_angle = self.goal.tilt_angle
+        self.fill_in_blindspot_with_second_scan = self.goal.fill_in_blindspot_with_second_scan
+        self.fast_scan = self.goal.fast_scan
+        self.publish_to_map = self.goal.publish_to_map
+        self.scan_and_detect()
+
+    def scan_and_detect(self):
+        self.aruco_tf = None
+        self.predock_tf = None
+        pan_angle = -3.69
+        
+        markers = []
+        scan_point = JointTrajectoryPoint()
+        scan_point.time_from_start = rospy.Duration(3.0)
+        scan_point.positions = [self.tilt_angle, pan_angle]
+
+        trajectory_goal = FollowJointTrajectoryGoal()
+        trajectory_goal.trajectory.joint_names = ['joint_head_tilt', 'joint_head_pan']
+        trajectory_goal.trajectory.points = [scan_point]
+        trajectory_goal.trajectory.header.stamp = rospy.Time.now()
+        trajectory_goal.trajectory.header.frame_id = 'base_link'
+
+        self.trajectory_client.send_goal(trajectory_goal)
+        self.trajectory_client.wait_for_result()
+
+        self.feedback.pan_angle = pan_angle
+        self.server.publish_feedback(self.feedback)
+
+        for pan_angle in np.arange(-3.69, 1.66, 0.7):
+            time.sleep(3.0)
+            for i in range(20):
+                if self.markers:
+                    markers = self.markers
+                    break
+
+            rospy.loginfo("Markers found: {}".format(markers))
+            
+            if markers != []:
+                for marker in markers:
+                    if marker.id == self.aruco_id:
+                        self.aruco_found = True
+                        self.aruco_name = marker.text
+                        if self.publish_to_map:
+                            trans = self.tf2_buffer.lookup_transform('map', self.aruco_name, rospy.Time())
+                            self.aruco_tf = self.broadcast_tf(trans.transform, self.aruco_name, 'map')
+                            rospy.loginfo("{} pose published to tf".format(self.aruco_name))
+                            if self.aruco_name == 'docking_station':
+                                # Transform the docking station frame such that x-axis points out of the aruco plane and 0.5 m in the front of the dock
+                                # This facilitates passing the goal pose as this predock frame so that the robot can back up into the dock
+                                saved_pose = Transform()
+                                saved_pose.translation.x = 0.0
+                                saved_pose.translation.y = -0.45
+                                saved_pose.translation.z = 0.47
+                                saved_pose.rotation.x = -0.382
+                                saved_pose.rotation.y = -0.352
+                                saved_pose.rotation.z = -0.604
+                                saved_pose.rotation.w = 0.604
+                                tran = self.broadcast_tf(saved_pose, 'predock_pose', 'docking_station')
+                                self.tf2_broadcaster.sendTransform(tran)
+                                try:
+                                    trans = self.tf2_buffer.lookup_transform('map', 'predock_pose', rospy.Time(), rospy.Duration(1.0))
+                                    # Bring predock_frame at base_link level
+                                    angles = euler_from_quaternion([trans.transform.rotation.x,
+                                                                    trans.transform.rotation.y,
+                                                                    trans.transform.rotation.z,
+                                                                    trans.transform.rotation.w])
+                                    q = quaternion_from_euler(0, 0, angles[2])
+                                    trans.transform.translation.z = 0
+                                    trans.transform.rotation.x = q[0]
+                                    trans.transform.rotation.y = q[1]
+                                    trans.transform.rotation.z = q[2]
+                                    trans.transform.rotation.w = q[3]
+                                    trans.header.stamp = rospy.Time.now()
+                                    self.predock_tf = trans
+                                    rospy.loginfo("Published predock pose")
+                                except (tf2_ros.LookupException, tf2_ros.ConnectivityException, tf2_ros.ExtrapolationException):
+                                    rospy.loginfo("Could not publish pose to tf")
+                                    pass
+
+            if not self.aruco_found:
+                self.feedback.pan_angle = pan_angle
+                self.server.publish_feedback(self.feedback)
+
+                scan_point.time_from_start = rospy.Duration(3.0)
+                scan_point.positions = [self.tilt_angle, pan_angle]
+                trajectory_goal.trajectory.points = [scan_point]
+                trajectory_goal.trajectory.header.stamp = rospy.Time.now()
+                trajectory_goal.trajectory.header.frame_id = 'base_link'
+
+                self.trajectory_client.send_goal(trajectory_goal)
+                self.trajectory_client.wait_for_result()
+            else:
+                break
+        
+        time.sleep(2.0)    
+        self.result_cb(self.aruco_found, "after headscan")
+
+    def result_cb(self, aruco_found, str=None):
+        self.result.aruco_found = aruco_found
+        if aruco_found:    
+            rospy.loginfo("Aruco marker found")
+            self.server.set_succeeded(self.result)
+        else:
+            rospy.loginfo("Could not find aruco marker {}".format(str))
+            self.server.set_aborted(self.result)
+
+    def broadcast_tf(self, trans, name, ref):
+        t = TransformStamped()
+        t.header.stamp = rospy.Time.now()
+        t.header.frame_id = ref
+        t.child_frame_id = name
+        t.transform = trans
+        return t
+
+    def aruco_callback(self, msg):
+        self.markers = msg.markers
+
+    def main(self):
+        self.rate = 5.0
+        rate = rospy.Rate(self.rate)
+        
+        self.tf2_buffer = tf2_ros.Buffer()
+        self.listener = tf2_ros.TransformListener(self.tf2_buffer)
+        self.tf2_broadcaster = tf2_ros.TransformBroadcaster()
+
+        dock_pose = Pose()
+        predock_pose = Pose()
+        while not rospy.is_shutdown():
+            try:
+                self.aruco_tf.header.stamp = rospy.Time.now()
+                self.predock_tf.header.stamp = rospy.Time.now()
+                self.tf2_broadcaster.sendTransform(self.aruco_tf)
+                if self.aruco_name == 'docking_station':
+                    dock_pose.position.x = self.aruco_tf.transform.translation.x
+                    dock_pose.position.y = self.aruco_tf.transform.translation.y
+                    dock_pose.position.z = self.aruco_tf.transform.translation.z
+                    dock_pose.orientation.x = self.aruco_tf.transform.rotation.x
+                    dock_pose.orientation.y = self.aruco_tf.transform.rotation.y
+                    dock_pose.orientation.z = self.aruco_tf.transform.rotation.z
+                    dock_pose.orientation.w = self.aruco_tf.transform.rotation.w
+                    self.dock_pose_pub.publish(dock_pose)
+                    self.tf2_broadcaster.sendTransform(self.predock_tf)
+                    predock_pose.position.x = self.predock_tf.transform.translation.x
+                    predock_pose.position.y = self.predock_tf.transform.translation.y
+                    predock_pose.position.z = self.predock_tf.transform.translation.z
+                    predock_pose.orientation.x = self.predock_tf.transform.rotation.x
+                    predock_pose.orientation.y = self.predock_tf.transform.rotation.y
+                    predock_pose.orientation.z = self.predock_tf.transform.rotation.z
+                    predock_pose.orientation.w = self.predock_tf.transform.rotation.w
+                    self.predock_pose_pub.publish(predock_pose)
+            except AttributeError:
+                pass
+            rate.sleep()
+
+
+def main():
+    try:
+        node = ArucoHeadScan()
+        node.main()
+        rospy.spin()
+    except KeyboardInterrupt:
+        print('interrupt received, so shutting down')
+
+
+if __name__ == '__main__':
+    main()

stretch_demos/nodes/autodocking_bt.py

@@ -0,0 +1,119 @@
+#!/usr/bin/env python3
+
+import py_trees
+import py_trees_ros
+import py_trees.console as console
+import rospy
+import sys
+import functools
+from stretch_demos.autodocking_behaviours import MoveBaseActionClient
+from stretch_demos.autodocking_behaviours import CheckTF
+from stretch_demos.autodocking_behaviours import VisualServoing
+from stretch_demos.msg import ArucoHeadScanAction, ArucoHeadScanGoal
+from geometry_msgs.msg import Pose
+from sensor_msgs.msg import BatteryState
+import hello_helpers.hello_misc as hm
+
+
+class AutodockingBT(hm.HelloNode):
+    def __init__(self):
+        hm.HelloNode.__init__(self)
+
+    def create_root(self):
+        # behaviours
+        
+        autodocking_seq_root = py_trees.composites.Sequence("autodocking")
+        dock_found_fb = py_trees.composites.Selector("dock_found_fb")
+        at_predock_fb = py_trees.composites.Selector("at_predock_fb")
+        charging_fb = py_trees.composites.Selector("charging_fb")
+
+        predock_found_sub = py_trees_ros.subscribers.CheckData(
+            name="predock_found_sub?",
+            topic_name='/predock_pose',
+            expected_value=None,
+            topic_type=Pose,
+            fail_if_no_data=True,fail_if_bad_comparison=False)
+        
+        is_charging_sub = py_trees_ros.subscribers.CheckData(
+            name="battery_charging?",
+            topic_name='/battery',
+            variable_name='present',
+            expected_value=True,
+            topic_type=BatteryState,
+            fail_if_no_data=True,fail_if_bad_comparison=True)
+
+        # at_predock_tf = CheckTF(self.tf2_buffer, name="at_predock_tf", target_frame='predock_pose')
+
+        aruco_goal = ArucoHeadScanGoal()
+        aruco_goal.aruco_id = 245
+        aruco_goal.tilt_angle = -0.68
+        aruco_goal.publish_to_map = True
+        aruco_goal.fill_in_blindspot_with_second_scan = False
+        aruco_goal.fast_scan = False
+        head_scan_action = py_trees_ros.actions.ActionClient( # Publishes predock pose to /predock_pose topic and tf frame called /predock_pose
+            name="ArucoHeadScan",
+            action_namespace="ArucoHeadScan",
+            action_spec=ArucoHeadScanAction,
+            action_goal=aruco_goal,
+            override_feedback_message_on_running="rotating"
+        )
+
+        predock_action = MoveBaseActionClient(
+            self.tf2_buffer,
+            name="predock_action",
+            override_feedback_message_on_running="moving"
+        )
+        invert_predock = py_trees.decorators.SuccessIsFailure(name='invert_predock', child=predock_action)
+
+        dock_action = VisualServoing(
+            name='dock_action',
+            source_frame='docking_station',
+            target_frame='charging_port',
+            override_feedback_message_on_running="docking"
+        )
+
+        # tree
+        autodocking_seq_root.add_children([dock_found_fb, at_predock_fb, dock_action, charging_fb])
+        dock_found_fb.add_children([predock_found_sub, head_scan_action])
+        at_predock_fb.add_children([predock_action])
+        charging_fb.add_children([is_charging_sub, invert_predock])
+        return autodocking_seq_root
+
+    def shutdown(self, behaviour_tree):
+        behaviour_tree.interrupt()
+
+    def main(self):
+        """
+        Entry point for the demo script.
+        """
+        hm.HelloNode.main(self, 'funmap', 'funmap')
+
+        root = self.create_root()
+        self.behaviour_tree = py_trees_ros.trees.BehaviourTree(root)
+        rospy.on_shutdown(functools.partial(self.shutdown, self.behaviour_tree))
+        if not self.behaviour_tree.setup(timeout=15):
+            console.logerror("failed to setup the tree, aborting.")
+            sys.exit(1)
+        
+        def print_tree(tree):
+            print(py_trees.display.unicode_tree(root=tree.root, show_status=True))
+
+        try:
+            self.behaviour_tree.tick_tock(
+                500
+                # period_ms=500,
+                # number_of_iterations=py_trees.trees.CONTINUOUS_TICK_TOCK,
+                # pre_tick_handler=None,
+                # post_tick_handler=print_tree
+            )
+        except KeyboardInterrupt:
+            self.behaviour_tree.interrupt()
+
+
+def main():
+    node = AutodockingBT()
+    node.main()
+
+
+if __name__ == '__main__':
+    main()

stretch_demos/nodes/visual_servoing_action.py

@@ -0,0 +1,182 @@
+#!/usr/bin/env python3
+
+import rospy
+from visualization_msgs.msg import MarkerArray
+from geometry_msgs.msg import Transform, TransformStamped, Pose
+import ros_numpy
+import numpy as np
+import tf2_ros
+import time
+import actionlib
+from math import sqrt, pow, acos, degrees
+from geometry_msgs.msg import Twist
+from trajectory_msgs.msg import JointTrajectoryPoint
+from control_msgs.msg import FollowJointTrajectoryGoal
+from tf.transformations import euler_from_quaternion, quaternion_from_euler
+from stretch_demos.msg import VisualServoAction, VisualServoGoal, VisualServoResult, VisualServoFeedback
+
+from sensor_msgs.msg import JointState
+from sensor_msgs.msg import PointCloud2
+import hello_helpers.hello_misc as hm
+
+class VisualServoing(hm.HelloNode):
+    def __init__(self):
+        hm.HelloNode.__init__(self)
+        hm.HelloNode.main(self, 'dock_visual_servo', 'dock_visual_servo', wait_for_first_pointcloud=False)
+
+        self.cmd_vel_pub = rospy.Publisher('/stretch/cmd_vel', Twist, queue_size=10)
+
+        self.server = actionlib.SimpleActionServer('autodock_visual_servo', VisualServoAction, self.execute_cb, False)
+        self.goal = VisualServoGoal()
+        self.feedback = VisualServoFeedback()
+        self.result = VisualServoResult()
+        self.cmd_vel = Twist()
+        self.cmd_vel.linear.x = 0.0
+        self.cmd_vel.linear.y = 0.0
+        self.cmd_vel.linear.z = 0.0
+        self.cmd_vel.angular.x = 0.0
+        self.cmd_vel.angular.y = 0.0
+        self.cmd_vel.angular.z = 0.0
+        self.server.start()
+
+        self.distance = 0.0
+        self.angle = 0.0
+        self.theta = 0.0 # angle between baselink x-axis and predock_pose x-axis
+        self.phi = 0.0 # angle between line connecting docking station and predock_pose and line connecting docking station with base_link
+
+    def execute_cb(self, goal):
+        self.goal = goal
+
+        scan_point = JointTrajectoryPoint()
+        scan_point.time_from_start = rospy.Duration(2.0)
+        scan_point.positions = [-1.051, -2.914] # Look back and down at the docking station
+
+        trajectory_goal = FollowJointTrajectoryGoal()
+        trajectory_goal.trajectory.joint_names = ['joint_head_tilt', 'joint_head_pan']
+        trajectory_goal.trajectory.points = [scan_point]
+        trajectory_goal.trajectory.header.stamp = rospy.Time.now()
+        trajectory_goal.trajectory.header.frame_id = 'base_link'
+
+        self.trajectory_client.send_goal(trajectory_goal)
+        self.trajectory_client.wait_for_result()
+        
+        while self.distance > 0.1 and self.distance < 0.5:
+            try:
+                predock_trans = self.tf2_buffer.lookup_transform('docking_station', 'predock_pose', rospy.Time(), rospy.Duration(1.0))
+                base_trans = self.tf2_buffer.lookup_transform('docking_station', 'base_link', rospy.Time(), rospy.Duration(1.0))
+                base_predock_trans = self.tf2_buffer.lookup_transform('predock_pose', 'base_link', rospy.Time(), rospy.Duration(1.0))
+
+                x = predock_trans.transform.translation.x
+                y = predock_trans.transform.translation.y
+                z = predock_trans.transform.translation.z
+                side_a = sqrt(pow(x, 2) + pow(y, 2) + pow(z, 2))
+
+                x = base_trans.transform.translation.x
+                y = base_trans.transform.translation.y
+                z = base_trans.transform.translation.z
+                side_b = sqrt(pow(x, 2) + pow(y, 2) + pow(z, 2))
+
+                x = base_predock_trans.transform.translation.x
+                y = base_predock_trans.transform.translation.y
+                z = base_predock_trans.transform.translation.z
+                side_c = sqrt(pow(x, 2) + pow(y, 2) + pow(z, 2))
+
+                if y < 0:
+                    self.phi = -acos((pow(side_a, 2) + pow(side_b, 2) - pow(side_c, 2))/(2.0 * side_a * side_b))
+                else:
+                    self.phi = acos((pow(side_a, 2) + pow(side_b, 2) - pow(side_c, 2))/(2.0 * side_a * side_b))
+
+                angles = euler_from_quaternion([base_predock_trans.transform.rotation.x,
+                                                    base_predock_trans.transform.rotation.y,
+                                                    base_predock_trans.transform.rotation.z,
+                                                    base_predock_trans.transform.rotation.w])
+                self.theta = -angles[2]
+                
+                self.angle = self.phi + self.theta
+                rospy.loginfo("Angle obtained: {}".format(self.angle))
+
+                # if self.angle > 0.05 or self.angle < -0.05:
+                if self.angle > (self.theta + 0.05) or self.angle < (self.theta - 0.05):
+                    self.cmd_vel.angular.z = self.angle*0.5
+                else:
+                    self.cmd_vel.angular.z = 0.0
+                self.cmd_vel.linear.x = -(self.distance*0.25)
+                self.cmd_vel_pub.publish(self.cmd_vel)
+                self.feedback.angle_error = self.angle
+                self.feedback.distance_error = self.distance
+                self.server.publish_feedback(self.feedback)
+            except:
+                self.cmd_vel.linear.x = 0.0
+                self.cmd_vel.angular.z = 0.0
+                self.cmd_vel_pub.publish(self.cmd_vel)
+
+        if self.distance > 0.5:
+            self.cmd_vel.linear.x = 0.0
+            self.cmd_vel.angular.z = 0.0
+            self.cmd_vel_pub.publish(self.cmd_vel)
+            self.result.result = False
+            self.result_cb(self.result.result)
+        else:
+            self.cmd_vel.linear.x = 0.0
+            self.cmd_vel.angular.z = 0.0
+            self.cmd_vel_pub.publish(self.cmd_vel)
+            self.result.result = True
+            self.result_cb(self.result.result)
+        
+
+    def result_cb(self, result):
+        if result:    
+            rospy.loginfo("Docking complete")
+            self.server.set_succeeded(self.result)
+        else:
+            rospy.loginfo("Failed to dock")
+            self.server.set_aborted(self.result)
+
+    def aruco_callback(self, msg):
+        self.markers = msg.markers
+
+    def main(self):
+        self.rate = 10.0
+        rate = rospy.Rate(self.rate)
+        
+        self.tf2_buffer = tf2_ros.Buffer()
+        self.listener = tf2_ros.TransformListener(self.tf2_buffer)
+        self.tf2_broadcaster = tf2_ros.TransformBroadcaster()
+
+        charging_port = TransformStamped()
+        charging_port.transform.translation.x = -0.3
+        charging_port.transform.translation.y = 0.0
+        charging_port.transform.translation.z = 0.1
+        charging_port.transform.rotation.x = 0.0
+        charging_port.transform.rotation.y = 0.0
+        charging_port.transform.rotation.z = 1.0
+        charging_port.transform.rotation.w = 0.0
+        charging_port.header.frame_id = 'base_link'
+        charging_port.child_frame_id = 'charging_port'
+        time.sleep(1.0)
+        while not rospy.is_shutdown():
+            try:
+                charging_port.header.stamp = rospy.Time.now()
+                self.tf2_broadcaster.sendTransform(charging_port)
+                transform = self.tf2_buffer.lookup_transform('charging_port', 'docking_station', rospy.Time(), rospy.Duration(1.0))
+                x = transform.transform.translation.x
+                y = transform.transform.translation.y
+                z = transform.transform.translation.z
+                self.distance = sqrt(pow(x, 2) + pow(y, 2) + pow(z, 2))
+                # rospy.loginfo("distance to docking_station {}".format(self.distance))
+            except (AttributeError, tf2_ros.LookupException, tf2_ros.ExtrapolationException):
+                pass
+            rate.sleep()
+
+
+def main():
+    try:
+        node = VisualServoing()
+        node.main()
+        rospy.spin()
+    except KeyboardInterrupt:
+        print('interrupt received, so shutting down')
+
+
+if __name__ == '__main__':
+    main()

stretch_demos/package.xml

@@ -69,6 +69,7 @@
   <build_export_depend>std_srvs</build_export_depend>
   <build_export_depend>tf</build_export_depend>
   <build_export_depend>tf2</build_export_depend>
+  <exec_depend>message_generation</exec_depend>
   <exec_depend>actionlib</exec_depend>
   <exec_depend>actionlib_msgs</exec_depend>
   <exec_depend>geometry_msgs</exec_depend>
@@ -81,6 +82,8 @@
   <exec_depend>tf</exec_depend>
   <exec_depend>tf2</exec_depend>
   <exec_depend>ros_numpy</exec_depend>
+  <build_depend>py_trees_ros</build_depend>
+  <build_depend>rqt_py_trees</build_depend>
 
 
   <!-- The export tag contains other, unspecified, tags -->

stretch_demos/setup.py

@@ -0,0 +1,8 @@
+#!/usr/bin/env python3
+
+from distutils.core import setup
+from catkin_pkg.python_setup import generate_distutils_setup
+
+setup_args = generate_distutils_setup( packages=['stretch_demos'], package_dir={'': 'src'})
+
+setup(**setup_args)

stretch_demos/src/stretch_demos/autodocking_behaviours.py

@@ -0,0 +1,180 @@
+#!/usr/bin/env python3
+
+import rospy
+import actionlib
+import py_trees
+from actionlib_msgs.msg import GoalStatus
+from math import sqrt, pow
+from geometry_msgs.msg import Pose, TransformStamped
+from move_base_msgs.msg import MoveBaseAction, MoveBaseGoal
+from stretch_demos.msg import VisualServoAction, VisualServoGoal, VisualServoResult, VisualServoFeedback
+import tf2_ros
+
+class MoveBaseActionClient(py_trees.behaviour.Behaviour):
+    def __init__(self, tf2_buffer, name="ActionClient", override_feedback_message_on_running="moving"):
+        super(MoveBaseActionClient, self).__init__(name)
+        self.action_client = None
+        self.sent_goal = False
+        self.action_spec = MoveBaseAction
+        self.action_goal = MoveBaseGoal()
+        self.action_namespace = "move_base"
+        self.override_feedback_message_on_running = override_feedback_message_on_running
+        self.tf2_buffer = tf2_buffer
+
+    def setup(self, timeout):
+        self.logger.debug("%s.setup()" % self.__class__.__name__)
+        self.action_client = actionlib.SimpleActionClient(
+            self.action_namespace,
+            self.action_spec
+        )
+        if not self.action_client.wait_for_server(rospy.Duration(timeout)):
+            self.logger.error("{0}.setup() could not connect to the action server at '{1}'".format(self.__class__.__name__, self.action_namespace))
+            self.action_client = None
+            return False
+        return True
+
+    def initialise(self):
+        # Update the goal data here
+        self.logger.debug("{0}.initialise()".format(self.__class__.__name__))
+        transform = self.tf2_buffer.lookup_transform('map', 'predock_pose', rospy.Time(), rospy.Duration(2.0))
+        goal_pose = Pose()
+        goal_pose.position.x = transform.transform.translation.x
+        goal_pose.position.y = transform.transform.translation.y
+        goal_pose.position.z = transform.transform.translation.z
+        goal_pose.orientation.x = transform.transform.rotation.x
+        goal_pose.orientation.y = transform.transform.rotation.y
+        goal_pose.orientation.z = transform.transform.rotation.z
+        goal_pose.orientation.w = transform.transform.rotation.w
+        self.action_goal.target_pose.header.frame_id = "map"
+        self.action_goal.target_pose.header.stamp = rospy.Time.now()
+        self.action_goal.target_pose.pose = goal_pose
+        self.sent_goal = False
+
+    def update(self):
+        self.logger.debug("{0}.update()".format(self.__class__.__name__))
+        if not self.action_client:
+            self.feedback_message = "no action client, did you call setup() on your tree?"
+            return py_trees.Status.INVALID
+        
+        if not self.sent_goal:
+            self.action_client.send_goal(self.action_goal)
+            self.sent_goal = True
+            self.feedback_message = "sent goal to the action server"
+            return py_trees.Status.RUNNING
+
+        self.feedback_message = self.action_client.get_goal_status_text()
+        if self.action_client.get_state() in [GoalStatus.ABORTED,
+                                              GoalStatus.PREEMPTED]:
+            return py_trees.Status.FAILURE
+        result = self.action_client.get_result()
+        if result:
+            return py_trees.Status.SUCCESS
+        else:
+            self.feedback_message = self.override_feedback_message_on_running
+            return py_trees.Status.RUNNING
+
+    def terminate(self, new_status):
+        self.logger.debug("%s.terminate()" % self.__class__.__name__)
+
+
+class CheckTF(py_trees.behaviour.Behaviour):
+    def __init__(self, tf2_buffer, name="CheckTF", source_frame="base_link", target_frame=None):
+        super(CheckTF, self).__init__(name)
+        self.source_frame = source_frame
+        self.target_frame = target_frame
+        self.tf2_buffer = tf2_buffer
+
+    def setup(self, timeout):
+        self.logger.debug("%s.setup()" % self.__class__.__name__)
+        return True
+
+    def initialise(self):
+        # Update the goal data here
+        self.logger.debug("{0}.initialise()".format(self.__class__.__name__))
+        transform = TransformStamped()
+        try:
+            transform = self.tf2_buffer.lookup_transform(self.source_frame, self.target_frame, rospy.Time(), rospy.Duration(1.0))
+            x = transform.transform.translation.x
+            y = transform.transform.translation.y
+            z = transform.transform.translation.z
+            self.error = sqrt(pow(x, 2) + pow(y, 2) + pow(z, 2))       
+        except tf2_ros.ExtrapolationException:
+            self.error = 10.0
+
+    def update(self):
+        """
+        Check only to see whether the underlying action server has
+        succeeded, is running, or has cancelled/aborted for some reason and
+        map these to the usual behaviour return states.
+        """
+        self.logger.debug("{0}.update()".format(self.__class__.__name__))
+        if self.error < 0.05:
+            return py_trees.Status.SUCCESS
+        else:
+            return py_trees.Status.FAILURE
+
+    def terminate(self, new_status):
+        self.logger.debug("%s.terminate()" % self.__class__.__name__)
+
+
+class VisualServoing(py_trees.behaviour.Behaviour):
+    def __init__(self, name="VisualServoing", source_frame="base_link", target_frame=None, override_feedback_message_on_running="servoing"):
+        super(VisualServoing, self).__init__(name)
+        self.action_client = None
+        self.sent_goal = False
+        self.action_spec = VisualServoAction
+        self.action_goal = VisualServoGoal()
+        self.action_namespace = "autodock_visual_servo"
+        self.source_frame = source_frame
+        self.target_frame = target_frame
+        self.override_feedback_message_on_running = override_feedback_message_on_running
+
+    def setup(self, timeout):
+        self.logger.debug("%s.setup()" % self.__class__.__name__)
+        self.action_client = actionlib.SimpleActionClient(
+            self.action_namespace,
+            self.action_spec
+        )
+        if not self.action_client.wait_for_server(rospy.Duration(timeout)):
+            self.logger.error("{0}.setup() could not connect to the action server at '{1}'".format(self.__class__.__name__, self.action_namespace))
+            self.action_client = None
+            return False
+        return True
+
+    def initialise(self):
+        # Update the goal data here
+        self.logger.debug("{0}.initialise()".format(self.__class__.__name__))
+        self.action_goal = VisualServoGoal()
+        self.action_goal.source_frame = self.source_frame
+        self.action_goal.target_frame = self.target_frame
+        
+    def update(self):
+        """
+        Check only to see whether the underlying action server has
+        succeeded, is running, or has cancelled/aborted for some reason and
+        map these to the usual behaviour return states.
+        """
+        self.logger.debug("{0}.update()".format(self.__class__.__name__))
+        if not self.action_client:
+            self.feedback_message = "no action client, did you call setup() on your tree?"
+            return py_trees.Status.INVALID
+        
+        if not self.sent_goal:
+            self.action_client.send_goal(self.action_goal)
+            self.sent_goal = True
+            self.feedback_message = "sent goal to the action server"
+            return py_trees.Status.RUNNING
+
+        self.feedback_message = self.action_client.get_goal_status_text()
+        if self.action_client.get_state() in [GoalStatus.ABORTED,
+                                              GoalStatus.PREEMPTED]:
+            return py_trees.Status.FAILURE
+        result = self.action_client.get_result()
+        if result:
+            return py_trees.Status.SUCCESS
+        else:
+            self.feedback_message = self.override_feedback_message_on_running
+            return py_trees.Status.RUNNING
+
+    def terminate(self, new_status):
+        self.logger.debug("%s.terminate()" % self.__class__.__name__)