Autodocking

src/leonardo_battery_guard/CMakeLists.txt

@@ -0,0 +1,12 @@
+cmake_minimum_required(VERSION 2.8.3)
+project(leonardo_battery_guard)
+
+find_package(catkin REQUIRED COMPONENTS kobuki_msgs actionlib_msgs diagnostic_msgs)
+
+catkin_package(CATKIN_DEPENDS kobuki_msgs actionlib_msgs diagnostic_msgs)
+
+catkin_install_python(
+  PROGRAMS
+    scripts/battery_guard.py
+    DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
+)

src/leonardo_battery_guard/launch/leonardo_battery_guard.launch

@@ -0,0 +1,6 @@
+
+<launch>
+    <node name="dock_drive_client" pkg="leonardo_battery_guard" type="battery_guard.py" clear_params="true" output="screen">
+        <param name="battery_threshold" value="10" />
+    </node>
+</launch>

src/leonardo_battery_guard/package.xml

@@ -0,0 +1,27 @@
+<package>
+  <name>leonardo_battery_guard</name>
+  <version>0.1.103</version>
+
+  <description>
+    Package that watch Leonardo's battery and dock it if necessary.
+  </description>
+  <maintainer email="[email protected]">Ernesto Corbellini</maintainer>
+  <maintainer email="[email protected]">Julian Mateu</maintainer>
+  <maintainer email="[email protected]">Patricio Tula</maintainer>
+  <author>Patricio Tula</author>
+  <license>BSD</license>
+
+  <buildtool_depend>catkin</buildtool_depend>
+  <build_depend>kobuki_msgs</build_depend>
+  <build_depend>actionlib_msgs</build_depend>
+  <build_depend>diagnostic_msgs</build_depend>
+
+  <run_depend>actionlib</run_depend>
+  <run_depend>roslib</run_depend>
+  <run_depend>rospy</run_depend>
+
+  <run_depend>kobuki_msgs</run_depend>
+  <run_depend>actionlib_msgs</run_depend>
+  <run_depend>diagnostic_msgs</run_depend>
+
+</package>

src/leonardo_battery_guard/scripts/battery_guard.py

@@ -0,0 +1,79 @@
+#!/usr/bin/env python
+#
+# Software License Agreement (BSD License)
+#
+# Description: This code creates a ros node in charge of watching the turtlebot Leonardo's batteries
+# and autodocking it in it's charging station if the battery level is too low. This code has been influenced by the kobuki
+# autodocking routine form https://github.com/yujinrobot/kobuki/blob/devel/kobuki_auto_docking/scripts/DockDriveActionClient.py
+#
+# author: Patricio Tula
+# reviewed by: Ernesto Corbellini / Julian Mateu
+
+import rospy
+
+import actionlib
+from kobuki_msgs.msg import AutoDockingAction, AutoDockingGoal
+from actionlib_msgs.msg import GoalStatus
+from diagnostic_msgs.msg import DiagnosticArray
+
+
+class AutoDocking(object):
+    def __init__(self):
+        self._ros_node = rospy.init_node("dock_drive_client", anonymous=True)
+        self._client = actionlib.SimpleActionClient("dock_drive_action", AutoDockingAction)
+        self._diagnostic_agg_sub = rospy.Subscriber("/diagnostics_agg", DiagnosticArray, self._listen_batteries)
+        self._doing_docking = False
+        self.BATTERY_THRESHOLD = rospy.get_param("~battery_threshold", 10)
+
+    def _doneCb(self, status, result):
+        """Callback that prints the action goal status and change the state of _doing_docking attribute. 
+        This function is called when the action goal is reached."""
+        if status == GoalStatus.PENDING: state="PENDING"
+        elif status == GoalStatus.ACTIVE: state="ACTIVE"
+        elif status == GoalStatus.PREEMPTED: state="PREEMPTED"
+        elif status == GoalStatus.SUCCEEDED: state="SUCCEEDED"
+        elif status == GoalStatus.ABORTED: state="ABORTED"
+        elif status == GoalStatus.REJECTED: state="REJECTED"
+        elif status == GoalStatus.PREEMPTING: state="PREEMPTING"
+        elif status == GoalStatus.RECALLING: state="RECALLING"
+        elif status == GoalStatus.RECALLED: state="RECALLED"
+        elif status == GoalStatus.LOST: state="LOST"
+        rospy.logdebug("Result - [ActionServer: " + state + "]: " + result.text)
+        self._doing_docking = False
+
+    def _feedbackCb(self, feedback):
+        """Callback that just prints the action feedback."""
+        rospy.logdebug("Feedback: [DockDrive: " + feedback.state + "]: " + feedback.text)
+
+    def _go_dock(self):
+        """Runs auto docking routine for a turtlebot."""
+        if rospy.is_shutdown(): 
+            return        
+        goal = AutoDockingGoal()
+        self._client.send_goal(goal, done_cb=self._doneCb, feedback_cb=self._feedbackCb)
+        rospy.logdebug("Goal: Sent.")
+        rospy.on_shutdown(self._client.cancel_goal)
+        self._doing_docking = False
+
+    def _listen_batteries(self, data):
+        """Callback that checks the batteries charge and triggers the auto docking routine if the charge is low."""
+        batteries_names = ["/Power System/Laptop Battery", "/Power System/Battery"]
+        batteries_values = [element.values for element in data.status if element.name in batteries_names]
+        laptop_battery_percentage = float(filter(lambda x: x.key == "Percentage (%)", batteries_values[0])[0].value)
+        kobuki_battery_percentage = float(filter(lambda x: x.key == "Percent", batteries_values[1])[0].value)
+
+        if self._doing_docking is False:
+            if kobuki_battery_percentage < self.BATTERY_THRESHOLD or laptop_battery_percentage < self.BATTERY_THRESHOLD:
+                self._go_dock()
+                self._doing_docking = True
+
+    def run(self):
+        rospy.spin()
+
+
+if __name__ == "__main__":
+    try:
+        autodock = AutoDocking()
+        autodock.run()
+    except rospy.ROSInterruptException: 
+        rospy.logdebug("program interrupted before completion")

src/leonardo_bringup/launch/leonardo.launch

@@ -4,5 +4,6 @@
   <include file="$(find turtlebot_teleop)/launch/xbox360_teleop.launch"/>
   <include file="$(find leonardo_bringup)/launch/image_convert.launch"/>
   <include file="$(find kobuki_auto_docking)/launch/minimal.launch"/>
+  <include file="$(find leonardo_battery_guard)/launch/leonardo_battery_guard.launch"/>
   <!-- <include file="/home/turtlebot/double_camera.launch"/> -->
 </launch>

src/leonardo_bringup/package.xml

@@ -18,5 +18,6 @@
   <run_depend>turtlebot_navigation</run_depend>
   <run_depend>turtlebot_teleop</run_depend>
   <run_depend>laptop_battery_monitor</run_depend>
+  <run_depend>leonardo_battery_guard</run_depend>
   <run_depend>image_proc</run_depend>
 </package>