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="20" />
+        <param name="position_goal" value="{'position': [-1.017, -0.414, 0.0], 'orientation':[0.0, 0.0, 0.95, 0.312]}" />
+    </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,133 @@
+#!/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 ast
+
+import actionlib
+from kobuki_msgs.msg import AutoDockingAction, AutoDockingGoal
+from actionlib_msgs.msg import GoalStatus
+from diagnostic_msgs.msg import DiagnosticArray
+from move_base_msgs.msg import MoveBaseAction, MoveBaseGoal
+from std_srvs.srv import Trigger, TriggerResponse
+
+
+class AutoDocking(object):
+    def __init__(self):
+        # Node
+        self._ros_node = rospy.init_node("dock_drive_client", anonymous=True)
+        # Services
+        self._service = rospy.Service('run_autodocking', Trigger, self._run_service)
+        # Action clients 
+        self._docking_client = actionlib.SimpleActionClient("dock_drive_action", AutoDockingAction)
+        self._docking_client.wait_for_server()
+        self._navigation_client = actionlib.SimpleActionClient('move_base', MoveBaseAction)
+        self._navigation_client.wait_for_server()
+        # Subscribed topics
+        self._diagnostic_agg_sub = rospy.Subscriber("/diagnostics_agg", DiagnosticArray, self._listen_batteries)
+        # Logic attributes
+        self._doing_docking = False
+        # Constants
+        self.BATTERY_THRESHOLD = rospy.get_param("~battery_threshold", 10)
+        self.POSITION_GOAL_DEFAULT = "{'position': [-1.017, -0.414, 0.0],'orientation':[0.0, 0.0, 0.95, 0.312]}"
+        position_goal = rospy.get_param("~position_goal", self.POSITION_GOAL_DEFAULT)
+        self.POSITION_GOAL = ast.literal_eval(position_goal)
+
+    def _run_service(self, req):
+        """Run docking routine"""
+        self._go_dock()
+        return TriggerResponse(True, "success")
+
+    def _parse_GoalStatus(self, status):
+        """Convert GoalStatus into strings"""
+        state = ""
+        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"
+        return state
+
+    def _done_docking(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."""
+        state = self._parse_GoalStatus(status)
+        rospy.logdebug("Docking - Result - [ActionServer: " + state + "]: " + result.text)
+        self._doing_docking = False
+
+    def _feedback_docking(self, feedback):
+        """Callback that just prints the action feedback."""
+        rospy.logdebug("Docking - Feedback: [DockDrive: " + feedback.state + "]: " + feedback.text)
+
+    def _done_navigating(self, status, result):
+        """Runs auto docking routine for a turtlebot."""
+        state = self._parse_GoalStatus(status)
+        rospy.logdebug("Navigation - Result - [ActionServer: " + str(state) + "]: " + str(result))
+        goal = AutoDockingGoal()
+        self._docking_client.send_goal(goal, done_cb=self._done_docking, feedback_cb=self._feedback_docking)
+        rospy.logdebug("Autodocking Goal: Sent.")
+        rospy.on_shutdown(self._docking_client.cancel_goal)
+
+    def _feedback_navigating(self, feedback):
+        """Callback that just prints the action feedback."""
+        rospy.logdebug("Navigation - Feedback: [Navigation: " + str(feedback.base_position) + "]")
+
+    def _go_dock(self):
+        """Runs auto docking routine for a turtlebot."""
+        if rospy.is_shutdown(): 
+            return
+        goal = MoveBaseGoal()
+        goal.target_pose.header.frame_id = "map"
+        goal.target_pose.header.stamp = rospy.Time.now()
+        goal.target_pose.pose.position.x = self.POSITION_GOAL["position"][0]
+        goal.target_pose.pose.position.y = self.POSITION_GOAL["position"][1]
+        goal.target_pose.pose.position.z = self.POSITION_GOAL["position"][2]
+        goal.target_pose.pose.orientation.x = self.POSITION_GOAL["orientation"][0]
+        goal.target_pose.pose.orientation.y = self.POSITION_GOAL["orientation"][1]
+        goal.target_pose.pose.orientation.z = self.POSITION_GOAL["orientation"][2]
+        goal.target_pose.pose.orientation.w = self.POSITION_GOAL["orientation"][3]
+        self._navigation_client.send_goal(goal, done_cb=self._done_navigating, feedback_cb=self._feedback_navigating)
+        rospy.logdebug("Navigation Goal: Sent.")
+        rospy.logdebug(goal)
+        rospy.on_shutdown(self._navigation_client.cancel_goal)
+
+    def _listen_batteries(self, data):
+        """Callback that checks the batteries charge and triggers the auto docking routine if the charge is low."""
+        if self._doing_docking is False:
+            batteries_names = ["/Power System/Laptop Battery", "/Power System/Battery"]
+            batteries_values = [element.values for element in data.status if element.name in batteries_names]
+            if len(batteries_values) == 2:
+                charging_state = str(filter(lambda x: x.key == "Charging State", batteries_values[1])[0].value)
+                batteries_charging = (charging_state != "Not Charging")
+                if batteries_charging is False:
+                    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 kobuki_battery_percentage < self.BATTERY_THRESHOLD or laptop_battery_percentage < self.BATTERY_THRESHOLD:
+                        self._go_dock()
+                        self._doing_docking = True
+
+    def run(self):
+        """Runs the node threat"""
+        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>