Merge remote-tracking branch 'origin/master' into PR/state_machine

.github/workflows/catkin_lint.yml

@@ -31,4 +31,4 @@ jobs:
           wstool init
           wstool merge aerial_robot_noetic.rosinstall
           wstool update
-          ROS_DISTRO=noetic catkin_lint --resolve-env --strict $PWD --skip-path kalman_filter --skip-path ublox_gps --skip-path aerial_robot_3rdparty  --skip-path rosserial --skip-path livox_ros_driver2 --skip-path fast_lio --skip-path mocap_optitrack
+          ROS_DISTRO=noetic catkin_lint --resolve-env --strict $PWD --skip-path kalman_filter --skip-path ublox_gps --skip-path aerial_robot_3rdparty  --skip-path rosserial --skip-path livox_ros_driver2 --skip-path fast_lio --skip-path mocap_optitrack --skip-path aerial_robot_base

aerial_robot_base/package.xml

@@ -1,5 +1,5 @@
 <?xml version="1.0"?>
-<package>
+<package format="3">
   <name>aerial_robot_base</name>
   <version>1.3.5</version>
   <description>The base for aerial robots</description>
@@ -17,18 +17,20 @@
   <build_depend>roscpp</build_depend>
   <build_depend>rospy</build_depend>
 
-  <run_depend>aerial_robot_control</run_depend>
-  <run_depend>aerial_robot_estimation</run_depend>
-  <run_depend>aerial_robot_model</run_depend>
-  <run_depend>joy</run_depend>
-  <run_depend>mocap_optitrack</run_depend>
-  <run_depend>smach_ros</run_depend>
-  <run_depend>smach_viewer</run_depend>
-  <run_depend>roscpp</run_depend>
-  <run_depend>ros_numpy</run_depend>
-  <run_depend>rospy</run_depend>
-  <run_depend>rostest</run_depend>
-  <run_depend>ublox_gps</run_depend>
-  <run_depend>ntrip_ros</run_depend>
+  <exec_depend>aerial_robot_control</exec_depend>
+  <exec_depend>aerial_robot_estimation</exec_depend>
+  <exec_depend>aerial_robot_model</exec_depend>
+  <exec_depend condition="$ROS_PYTHON_VERSION == 2">ipython</exec_depend>
+  <exec_depend condition="$ROS_PYTHON_VERSION == 3">ipython3</exec_depend>
+  <exec_depend>joy</exec_depend>
+  <exec_depend>mocap_optitrack</exec_depend>
+  <exec_depend>smach_ros</exec_depend>
+  <exec_depend>smach_viewer</exec_depend>
+  <exec_depend>roscpp</exec_depend>
+  <exec_depend>ros_numpy</exec_depend>
+  <exec_depend>rospy</exec_depend>
+  <exec_depend>rostest</exec_depend>
+  <exec_depend>ublox_gps</exec_depend>
+  <exec_depend>ntrip_ros</exec_depend>
 
 </package>

aerial_robot_base/src/aerial_robot_base/hovering_check.py

@@ -0,0 +1,122 @@
+#!/usr/bin/env python
+# -*- coding: utf-8 -*-
+
+# Software License Agreement (BSD License)
+
+# Copyright (c) 2019, JSK Robotics Laboratory, The University of Tokyo
+# All rights reserved.
+#
+# Redistribution and use in source and binary forms, with or without
+# modification, are permitted provided that the following conditions are met:
+#
+#     * Redistributions of source code must retain the above copyright
+#       notice, this list of conditions and the following disclaimer.
+#     * Redistributions in binary form must reproduce the above copyright
+#       notice, this list of conditions and the following disclaimer in the
+#       documentation and/or other materials provided with the distribution.
+#     * Neither the name of the Willow Garage, Inc. nor the names of its
+#       contributors may be used to endorse or promote products derived from
+#       this software without specific prior written permission.
+#
+# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+# AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+# IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
+# ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
+# LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
+# CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
+# SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
+# INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
+# CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
+# ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+# POSSIBILITY OF SUCH DAMAGE.
+
+from __future__ import print_function
+
+import sys
+import time
+import math
+import unittest
+
+import rospy
+import rostest
+from std_msgs.msg import Empty
+from aerial_robot_msgs.msg import PoseControlPid
+
+PKG = 'rostest'
+
+class HoveringCheck():
+    def __init__(self, name="hovering_check"):
+        self.hovering_duration = rospy.get_param('~hovering_duration', 0.0)
+        self.convergence_thresholds = rospy.get_param('~convergence_thresholds', [0.01, 0.01, 0.01]) # [xy,z, theta]
+        self.controller_sub = rospy.Subscriber('debug/pose/pid', PoseControlPid, self._controlCallback)
+        self.control_msg = None
+
+    def hoveringCheck(self):
+
+        # start motor arming
+        start_pub = rospy.Publisher('teleop_command/start', Empty, queue_size=1)
+        takeoff_pub = rospy.Publisher('teleop_command/takeoff', Empty, queue_size=1)
+
+        time.sleep(0.5) # wait for publisher initialization
+        start_pub.publish(Empty())
+
+        time.sleep(1.0) # second
+
+        # start takeoff
+        takeoff_pub.publish(Empty())
+
+        deadline = rospy.Time.now() + rospy.Duration(self.hovering_duration)
+        while not rospy.Time.now() > deadline:
+            if self.control_msg is not None:
+                err_x = self.control_msg.x.err_p;
+                err_y = self.control_msg.y.err_p;
+                err_z = self.control_msg.z.err_p;
+                err_yaw = self.control_msg.yaw.err_p;
+                err_xy = math.sqrt(err_x * err_x + err_y * err_y);
+                rospy.loginfo_throttle(1, 'errors in [xy, z, yaw]: [%f (%f, %f), %f, %f]' %  (err_xy, err_x, err_y, err_z, err_yaw))
+            rospy.sleep(1.0)
+
+        err_x = self.control_msg.x.err_p;
+        err_y = self.control_msg.y.err_p;
+        err_z = self.control_msg.z.err_p;
+        err_yaw = self.control_msg.yaw.err_p;
+        err_xy = math.sqrt(err_x * err_x + err_y * err_y);
+
+        # check convergence
+        if err_xy < self.convergence_thresholds[0] and math.fabs(err_z) < self.convergence_thresholds[1] and math.fabs(err_yaw) < self.convergence_thresholds[2]:
+            return True
+
+        # cannot be convergent
+        rospy.logerr('errors in [xy, z, yaw]: [%f (%f, %f), %f, %f]' %  (err_xy, err_x, err_y, err_z, err_yaw))
+
+        return False
+
+    def _controlCallback(self, msg):
+        self.control_msg = msg
+
+        err_x = self.control_msg.x.err_p;
+        err_y = self.control_msg.y.err_p;
+        err_z = self.control_msg.z.err_p;
+        err_yaw = self.control_msg.yaw.err_p;
+        err_xy = math.sqrt(err_x * err_x + err_y * err_y);
+        rospy.logdebug_throttle(1, 'errors in [xy, z, yaw]: [%f (%f, %f), %f, %f]' %  (err_xy, err_x, err_y, err_z, err_yaw))
+
+class HoveringTest(unittest.TestCase):
+    def __init__(self, *args):
+        super(self.__class__, self).__init__(*args)
+        rospy.init_node("hovering_test")
+        self.hovering_delay = rospy.get_param('~hovering_delay', 1.0)
+
+    def test_hovering(self):
+        rospy.sleep(self.hovering_delay)
+        checker = HoveringCheck()
+        self.assertTrue(checker.hoveringCheck(), msg='Cannot reach convergence')
+
+if __name__ == '__main__':
+    print("start check hovering")
+    try:
+        rostest.run(PKG, 'hovering_check', HoveringTest, sys.argv)
+    except KeyboardInterrupt:
+        pass
+
+    print("exiting")

aerial_robot_base/src/aerial_robot_base/robot_interface.py

@@ -514,3 +514,8 @@ def setJointTorque(self, state):
             self.set_joint_torque_client(req)
         except rospy.ServiceException as e:
             print("Service call failed: %s"%e)
+
+if __name__ == "__main__":
+      from IPython import embed
+      ri = RobotInterface(init_node=True)
+      embed()