From 7d4b34769746e0064c0b998eac2ad9fea04b62c1 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?=E8=B6=99=E3=80=80=E6=BC=A0=E5=B1=85=28Zhao=2C=20Moju=29?= Date: Wed, 15 Jan 2025 22:23:48 +0900 Subject: [PATCH] Handle robots with state machine (#656) * [RosTest] move the module of hovering check to "src" directory * [Robot Interface] add python interpreted interface to support basic navigation motion, including joint control * [Navigation] refactor the process for landing phase. Only accept new pose in hover state and also reset the target velocity in landing phase * [Dragon][Naviagation] refactor the landing process by introducing a new state: pre_land_state * [Dragon] remove unnecessary scripts * [State Machine] implement basic classes based on ros smach, and also crate a simple demo script. * [Demo] creates more simple demo based on smach and integrate the script in each bringup launch * [RosTest] refactor the flight control test script to be based on the new state machine system * [Demo] remove unnecessary old demo scripts --------- Co-authored-by: Moju Zhao --- aerial_robot_base/package.xml | 2 + aerial_robot_base/scripts/simple_demo.py | 95 ++++ .../src/aerial_robot_base/state_machine.py | 472 ++++++++++++++++++ aerial_robot_base/src/hovering_check.py | 112 +++++ robots/dragon/launch/bringup.launch | 4 + robots/dragon/scripts/simple_demo.py | 128 +++++ robots/dragon/scripts/transformation_demo.py | 51 -- .../scripts/transformation_demo_vertical.py | 42 -- robots/dragon/test/dragon_control.test | 109 ++-- robots/dragon/test/flight_check.py | 164 ++++++ robots/hydrus/launch/bringup.launch | 4 + robots/hydrus/scripts/hydrus_demo.py | 29 -- robots/hydrus/scripts/loop_demo.py | 115 +++++ robots/hydrus/scripts/simple_demo.py | 109 ++++ robots/hydrus/test/control_check.py | 245 --------- robots/hydrus/test/flight_check.py | 164 ++++++ robots/hydrus/test/hydrus_control.test | 49 +- robots/hydrus/test/tilted_hydrus_control.test | 45 +- robots/hydrus_xi/launch/bringup.launch | 4 + .../test/hydrus_xi_hex_branch_control.test | 26 +- robots/mini_quadrotor/launch/bringup.launch | 5 + .../mini_quadrotor/test/flight_control.test | 18 +- 22 files changed, 1477 insertions(+), 515 deletions(-) create mode 100755 aerial_robot_base/scripts/simple_demo.py create mode 100644 aerial_robot_base/src/aerial_robot_base/state_machine.py create mode 100755 aerial_robot_base/src/hovering_check.py create mode 100755 robots/dragon/scripts/simple_demo.py delete mode 100755 robots/dragon/scripts/transformation_demo.py delete mode 100755 robots/dragon/scripts/transformation_demo_vertical.py create mode 100755 robots/dragon/test/flight_check.py delete mode 100755 robots/hydrus/scripts/hydrus_demo.py create mode 100755 robots/hydrus/scripts/loop_demo.py create mode 100755 robots/hydrus/scripts/simple_demo.py delete mode 100755 robots/hydrus/test/control_check.py create mode 100755 robots/hydrus/test/flight_check.py diff --git a/aerial_robot_base/package.xml b/aerial_robot_base/package.xml index 859b32a64..722d40fbe 100644 --- a/aerial_robot_base/package.xml +++ b/aerial_robot_base/package.xml @@ -24,6 +24,8 @@ ipython3 joy mocap_optitrack + smach_ros + smach_viewer roscpp ros_numpy rospy diff --git a/aerial_robot_base/scripts/simple_demo.py b/aerial_robot_base/scripts/simple_demo.py new file mode 100755 index 000000000..784cee700 --- /dev/null +++ b/aerial_robot_base/scripts/simple_demo.py @@ -0,0 +1,95 @@ +#!/usr/bin/env python +# -*- coding: utf-8 -*- + +# Software License Agreement (BSD License) + +# Copyright (c) 2025, DRAGON 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. + +import rospy + +from aerial_robot_base.robot_interface import RobotInterface +from aerial_robot_base.state_machine import * + +# Template for simple demo +class SimpleDemo(): + def __init__(self): + self.robot = RobotInterface() + + self.sm_top = smach.StateMachine(outcomes=['succeeded', 'preempted']) + self.sm_top.userdata.flags = {} + self.sm_top.userdata.extra = {} + + with self.sm_top: + smach.StateMachine.add('Start', Start(self.robot), + transitions={'succeeded':'Arm', + 'preempted':'preempted'}, + remapping={'flags':'flags', 'extra':'extra'}) + + + smach.StateMachine.add('Arm', Arm(self.robot), + transitions={'succeeded':'Takeoff', + 'preempted':'preempted'}, + remapping={'flags':'flags', 'extra':'extra'}) + + smach.StateMachine.add('Takeoff', Takeoff(self.robot), + transitions={'succeeded':'WayPoint', + 'preempted':'preempted'}, + remapping={'flags':'flags', 'extra':'extra'}) + + + smach.StateMachine.add('WayPoint', WayPoint(self.robot, waypoints = [[0,0,1.5], [1,0,1]], hold_time = 2.0), + transitions={'succeeded':'Circle', + 'preempted':'preempted'}, + remapping={'flags':'flags', 'extra':'extra'}) + + smach.StateMachine.add('Circle', CircleTrajectory(self.robot, period = 20.0), + transitions={'succeeded':'Land', + 'preempted':'preempted'}, + remapping={'flags':'flags', 'extra':'extra'}) + + smach.StateMachine.add('Land', Land(self.robot), + transitions={'succeeded':'succeeded', + 'preempted':'preempted'}, + remapping={'flags':'flags', 'extra':'extra'}) + + self.sis = smach_ros.IntrospectionServer('task_smach_server', self.sm_top, '/SM_ROOT') + + self.sis.start() + outcome = self.sm_top.execute() + + self.sis.stop() + +if __name__ == '__main__': + + rospy.init_node('simple_demo') + + demo = SimpleDemo() + + + + diff --git a/aerial_robot_base/src/aerial_robot_base/state_machine.py b/aerial_robot_base/src/aerial_robot_base/state_machine.py new file mode 100644 index 000000000..b7f157a03 --- /dev/null +++ b/aerial_robot_base/src/aerial_robot_base/state_machine.py @@ -0,0 +1,472 @@ +#!/usr/bin/env python +# -*- coding: utf-8 -*- + +# Software License Agreement (BSD License) + +# Copyright (c) 2025, DRAGON 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. + +import rospy +import smach +import smach_ros +import functools +import numpy as np +from aerial_robot_base.robot_interface import RobotInterface +from std_msgs.msg import Empty +from sensor_msgs.msg import Joy + +class BaseState(smach.State): + def __init__(self, robot, outcomes=[], input_keys=[], output_keys=[], io_keys=['flags', 'extra']): + smach.State.__init__(self, outcomes, input_keys, output_keys, io_keys) + self.robot = robot + + def hold(self, t, flags): + + if 'interfere_mode' not in flags: + rospy.sleep(t) + else: + interfere_mode = flags['interfere_mode'] + + if not interfere_mode: + rospy.sleep(t) + else: + message = '\n\n Please press "L1" and "R1" at the same time to proceed the state \n' + rospy.loginfo('\033[32m'+ message +'\033[0m') + + while not rospy.is_shutdown(): + + if flags['interfering']: + flags['interfering'] = False # reset for subsequent state + break + + rospy.sleep(0.1) + + + +# trigger to start state machine +class Start(BaseState): + def __init__(self, robot): + BaseState.__init__(self, robot, outcomes=['succeeded', 'preempted']) + + self.flags = {'interfere_mode': False, 'interfering': False} + + self.task_start = False + self.task_start_sub = rospy.Subscriber('/task_start', Empty, self.taskStartCallback) + self.joy_sub = rospy.Subscriber(self.robot.robot_ns + '/joy', Joy, self.joyCallback) + + def taskStartCallback(self, msg): + self.task_start = True + + def joyCallback(self, msg): + + # Check interfering from joy + if len(msg.axes) == 14 and len(msg.buttons) == 14: + # This is PS4 Joy Controller + if msg.buttons[4] == 1 and msg.buttons[5] == 1: + + if not self.flags['interfere_mode']: + rospy.loginfo("Enter interfere mode") + self.flags['interfere_mode'] = True + + self.flags['interfering'] = True + else: + self.flags['interfering'] = False + + def execute(self, userdata): + + message = \ + '\n\n Please run following command to start the state machine: \n' + \ + ' "$ rostopic pub -1 /task_start std_msgs/Empty" \n \n' + \ + ' Or you can press "L1" and "R1" at the same time to enter interfere mode, \n' + \ + ' which need to use controller like PS4 to manually proceed the state. \n' + rospy.loginfo('\033[32m'+ message +'\033[0m') + + userdata.flags = self.flags + + while not self.task_start: + rospy.sleep(0.1) + rospy.logdebug_throttle(1.0, "wait to start task") + + if userdata.flags['interfere_mode']: + rospy.loginfo(self.__class__.__name__ + ': enter the inteferece mode') + userdata.flags['interfering'] = False # reset for subsequent state + break + + if rospy.is_shutdown(): + print("shutdown ros") + return 'preempted' + + pass + + rospy.loginfo("start task") + return 'succeeded' + +# the template state for arm, takeoff, land +class SingleCommandState(BaseState): + def __init__(self, robot, prefix, func, start_flight_state, target_flight_state, timeout, hold_time): + BaseState.__init__(self, robot, outcomes=['succeeded', 'preempted']) + + self.prefix = prefix + self.func = func + self.start_flight_state = start_flight_state + self.target_flight_state = target_flight_state + self.hold_time = rospy.get_param('~' + self.prefix + '/hold_time', hold_time) + self.timeout = rospy.get_param('~' + self.prefix + '/timeout', timeout) + + def execute(self, userdata): + + flight_state = self.robot.getFlightState() + + if flight_state != self.start_flight_state: + rospy.logwarn(self.__class__.__name__ + ': the robot state ({}) is not at the start_flight_state ({}). preempted!'.format(flight_state, self.start_flight_state)) + return 'preempted' + + rospy.loginfo(self.__class__.__name__ + ': start {}'.format(self.prefix)) + self.func() + + start_t = rospy.get_time() + while rospy.get_time() < start_t + self.timeout: + + flight_state = self.robot.getFlightState() + if flight_state == self.target_flight_state: + rospy.loginfo(self.__class__.__name__ + ': robot succeed to {}!'.format(self.prefix)) + self.hold(self.hold_time, userdata.flags) + return 'succeeded' + + if rospy.is_shutdown(): + print("shutdown ros") + return 'preempted' + + rospy.logwarn(self.__class__.__name__ + ': timeout ({}sec). preempted!'.format(self.timeout)) + return 'preempted' + + +class Arm(SingleCommandState): + def __init__(self, robot): + SingleCommandState.__init__(self, robot, 'arm', robot.start, robot.ARM_OFF_STATE, robot.ARM_ON_STATE, 2.0, 2.0) + + def execute(self, userdata): + + flight_state = self.robot.getFlightState() + + if flight_state == self.robot.HOVER_STATE: + rospy.loginfo(self.__class__.__name__ + ': robot already hovers, skip') + return 'succeeded' + + return SingleCommandState.execute(self, userdata) + +class Takeoff(SingleCommandState): + def __init__(self, robot): + SingleCommandState.__init__(self, robot, 'takeoff', robot.takeoff, robot.ARM_ON_STATE, robot.HOVER_STATE, 30.0, 2.0) + + def execute(self, userdata): + + flight_state = self.robot.getFlightState() + + if flight_state == self.robot.HOVER_STATE: + rospy.loginfo(self.__class__.__name__ + ': robot already hovers, skip') + return 'succeeded' + + return SingleCommandState.execute(self, userdata) + +class Land(SingleCommandState): + def __init__(self, robot): + SingleCommandState.__init__(self, robot, 'land', robot.land, robot.HOVER_STATE, robot.ARM_OFF_STATE, 20.0, 0) + + +# waypoint +class WayPoint(BaseState): + def __init__(self, robot, prefix = 'waypoint', waypoints = [], timeout = 30.0, hold_time = 1.0): + BaseState.__init__(self, robot, outcomes=['succeeded', 'preempted']) + + self.waypoints = rospy.get_param('~' + prefix + '/waypoints', waypoints) + self.timeout = rospy.get_param('~' + prefix + '/timeout', timeout) + self.hold_time = rospy.get_param('~' + prefix + '/hold_time', hold_time) + self.pos_thresh = rospy.get_param('~' + prefix+ '/pos_thresh', 0.1) + self.vel_thresh = rospy.get_param('~' + prefix + '/vel_thresh', 0.05) + self.yaw_thresh = rospy.get_param('~' + prefix + '/yaw_thresh', 0.1) + + def execute(self, userdata): + + flight_state = self.robot.getFlightState() + + if flight_state != self.robot.HOVER_STATE: + rospy.logwarn(self.__class__.__name__ + ': the robot state ({}) is not at HOVER_STATE. preempted!'.format(flight_state)) + return 'preempted' + + if len(self.waypoints) == 0: + rospy.logwarn(self.__class__.__name__ + ': the waypoints is empty. preempted') + return 'preempted' + + for i, waypoint in enumerate(self.waypoints): + + if len(waypoint) == 3: + # only position + ret = self.robot.goPos(pos = waypoint, pos_thresh = self.pos_thresh, vel_thresh = self.vel_thresh, timeout = self.timeout) + elif len(waypoint) == 4: + # position + yaw + ret = self.robot.goPosYaw(pos = waypoint[:3], yaw = waypoint[-1], pos_thresh = self.pos_thresh, vel_thresh = self.vel_thresh, yaw_thresh = self.yaw_thresh, timeout = self.timeout) + else: + rospy.logwarn(self.__class__.__name__ + ': the format of waypoint {} is not supported, the size should be either 3 and 4. preempted!'.format(waypoint)) + return 'preempted' + + if ret: + rospy.loginfo(self.__class__.__name__ + ': robot succeeds to reach the {}th waypoint [{}]'.format(i+1, waypoint)) + self.hold(self.hold_time, userdata.flags) + else: + rospy.logwarn(self.__class__.__name__ + ': robot cannot reach {}. preempted'.format(waypoint)) + return 'preempted' + + + return 'succeeded' + + +class CircleTrajectory(BaseState): + def __init__(self, robot, period = 20.0, radius = 1.0, init_theta = 0.0, yaw = True, loop = 1, timeout = 30.0, hold_time = 2.0): + BaseState.__init__(self, robot, outcomes=['succeeded', 'preempted']) + + self.period = rospy.get_param("~circle/period", period) + self.radius = rospy.get_param("~circle/radius", radius) + self.init_theta = rospy.get_param("~circle/init_theta", init_theta) + self.yaw = rospy.get_param("~circle/yaw", yaw) + self.loop = rospy.get_param("~circle/loop", loop) + self.hold_time = rospy.get_param('~circle/hold_time', hold_time) + + self.omega = 2 * np.pi / self.period + self.velocity = self.omega * self.radius + + self.nav_rate = rospy.get_param("~nav_rate", 20.0) # hz + self.nav_rate = 1 / self.nav_rate + + def execute(self, userdata): + + current_pos = self.robot.getCogPos() + center_pos_x = current_pos[0] - np.cos(self.init_theta) * self.radius + center_pos_y = current_pos[1] - np.sin(self.init_theta) * self.radius + center_pos_z = current_pos[2] + + init_yaw = self.robot.getCogRPY()[2] + + rospy.loginfo("the center position is [%f, %f]", center_pos_x, center_pos_y) + + loop = 0 + cnt = 0 + + while loop < self.loop: + + if self.robot.flight_state != self.robot.HOVER_STATE: + rospy.logerr("[Circle Motion] the robot is not hovering, preempt!") + return 'preempted' + + theta = self.init_theta + cnt * self.nav_rate * self.omega + + pos_x = center_pos_x + np.cos(theta) * self.radius + pos_y = center_pos_y + np.sin(theta) * self.radius + pos = [pos_x, pos_y, center_pos_z] + vel_x = -np.sin(theta) * self.velocity + vel_y = np.cos(theta) * self.velocity + vel = [vel_x, vel_y, 0] + + if self.yaw: + yaw = init_yaw + cnt * self.nav_rate * self.omega + rot = [0, 0, yaw] + ang_vel = [0, 0, self.omega] + self.robot.navigate(pos = pos, rot = rot, lin_vel = vel, ang_vel = ang_vel) + else: + self.robot.navigate(pos = pos, lin_vel = vel) + + cnt += 1 + + if cnt == self.period // self.nav_rate: + loop += 1 + cnt = 0 + + rospy.sleep(self.nav_rate) + + # stop + self.robot.navigate(lin_vel = [0,0,0], ang_vel = [0,0,0]) + self.hold(self.hold_time, userdata.flags) + + return 'succeeded' + + +# Joint +class FormCheck(BaseState): + def __init__(self, robot, prefix = 'form_check', joint_names = [], joint_angles = [], thresh = 0.02, timeout = 10.0): + BaseState.__init__(self, robot, outcomes=['succeeded', 'preempted']) + + self.target_joint_names = rospy.get_param('~' + prefix + '/joint_names', joint_names) + self.target_joint_angles = rospy.get_param('~' + prefix + '/joint_angles', joint_angles) + self.timeout = rospy.get_param('~' + prefix + '/timeout', timeout) + self.thresh = rospy.get_param('~' + prefix + '/angle_thresh', thresh) + + def execute(self, userdata): + + # check convergence + ret = self.robot.jointConvergenceCheck(self.timeout, self.target_joint_names, self.target_joint_angles, self.thresh) + + if ret: + rospy.loginfo(self.__class__.__name__ + ': robot succeed to convernge to the target joints {}:{}!'.format(self.target_joint_names, self.target_joint_angles)) + return 'succeeded' + else: + rospy.logwarn(self.__class__.__name__ + ': timeout ({}sec). preempted!'.format(self.timeout)) + return 'preempted' + + +class Transform(BaseState): + def __init__(self, robot, prefix = 'transform', joint_names = [], joint_trajectory = [], thresh = 0.05, timeout = 10.0, hold_time = 2.0): + BaseState.__init__(self, robot, outcomes=['succeeded', 'preempted']) + + self.target_joint_names = rospy.get_param('~' + prefix + '/joint_names', joint_names) + self.target_joint_trajectory = rospy.get_param('~' + prefix + '/joint_trajectory', joint_trajectory) + self.timeout = rospy.get_param('~' + prefix + '/timeout', timeout) + self.hold_time = rospy.get_param('~' + prefix + '/hold_time', hold_time) + self.thresh = rospy.get_param('~' + prefix + '/angle_thresh', thresh) + + + def execute(self, userdata): + + flight_state = self.robot.getFlightState() + + if flight_state != self.robot.HOVER_STATE: + rospy.logwarn(self.__class__.__name__ + ': the robot state ({}) is not at HOVER_STATE. preempted!'.format(flight_state)) + return 'preempted' + + if len(self.target_joint_trajectory) == 0: + rospy.logwarn(self.__class__.__name__ + ': the joint trajectory is empty. preempted') + return 'preempted' + + for i, target_angles in enumerate(self.target_joint_trajectory): + ret = self.robot.setJointAngle(self.target_joint_names, target_angles, self.thresh, self.timeout) + + if ret: + rospy.loginfo(self.__class__.__name__ + ': robot succeed to convernge to the {} th target joints {}:{}!'.format(i + 1, self.target_joint_names, target_angles)) + self.hold(self.hold_time, userdata.flags) + else: + rospy.logwarn(self.__class__.__name__ + ': timeout ({}sec), fail to reach the {} th target joints. preempted!'.format(i+1, self.timeout)) + return 'preempted' + + + return 'succeeded' + +# Joint + Pose +class TransformWithPose(BaseState): + def __init__(self, robot, prefix = 'motion', joint_names = [], joint_trajectory = [], pos_trajectory = [], rot_trajectory = [], joint_thresh = 0.05, pos_thresh = 0.1, rot_thresh = 0.1, timeout = 10.0, hold_time = 2.0, rotate_cog = False): + BaseState.__init__(self, robot, outcomes=['succeeded', 'preempted']) + + self.target_joint_names = rospy.get_param('~' + prefix + '/joint_names', joint_names) + self.target_joint_trajectory = rospy.get_param('~' + prefix + '/joint_trajectory', joint_trajectory) + self.target_pos_trajectory = rospy.get_param('~' + prefix + '/pos_trajectory', pos_trajectory) + self.target_rot_trajectory = rospy.get_param('~' + prefix + '/rot_trajectory', rot_trajectory) + self.timeout = rospy.get_param('~' + prefix + '/timeout', timeout) + self.hold_time = rospy.get_param('~' + prefix + '/hold_time', hold_time) + self.joint_thresh = rospy.get_param('~' + prefix + '/joint_thresh', joint_thresh) + self.pos_thresh = rospy.get_param('~' + prefix + '/pos_thresh', pos_thresh) + self.rot_thresh = rospy.get_param('~' + prefix + '/rot_thresh', rot_thresh) + self.rotate_cog = rospy.get_param('~' + prefix + '/rotate_cog', rotate_cog) # speical rotation for CoG + + def execute(self, userdata): + + flight_state = self.robot.getFlightState() + + if flight_state != self.robot.HOVER_STATE: + rospy.logwarn(self.__class__.__name__ + ': the robot state ({}) is not at HOVER_STATE. preempted!'.format(flight_state)) + return 'preempted' + + if len(self.target_joint_trajectory) == 0: + rospy.logwarn(self.__class__.__name__ + ': the joint trajectory is empty. preempted') + return 'preempted' + + for i, target_angles in enumerate(self.target_joint_trajectory): + + # send target position + target_pos = None + if len(self.target_pos_trajectory) > 0: + + if len(self.target_pos_trajectory) != len(self.target_joint_trajectory): + rospy.logwarn(self.__class__.__name__ + ': the size of joint trajectory and that of pos is not equal. preempted!') + return 'preempted' + + target_pos = self.target_pos_trajectory[i] + self.robot.goPos(target_pos, timeout = 0) + + # send target rotation + target_rot = None + if len(self.target_rot_trajectory) > 0: + if len(self.target_rot_trajectory) != len(self.target_joint_trajectory): + rospy.logwarn(self.__class__.__name__ + ': the size of joint trajectory and that of rot is not equal. preempted!') + return 'preempted' + + target_rot = self.target_rot_trajectory[i] + + if self.rotate_cog: + + if len(target_rot) > 3 or len(target_rot) < 2: + rospy.logwarn(self.__class__.__name__ + ': the size of target rot should be 2 or 3 for special cog ratate mode. preempted!') + return 'preempted' + + self.robot.rotateCog(target_rot[0], target_rot[1]) + + if len(target_rot) == 3: + self.robot.rotateyaw(target_rot[2], timeout = 0) + else: + # fill the yaw element for subsequent pose convergence check + curr_yaw = self.robot.getCogRPY()[2] + target_rot.append(curr_yaw) + + else: + self.robot.rotate(target_rot, timeout = 0) + + start_time = rospy.get_time() + + # send joint angles + rospy.loginfo(self.__class__.__name__ + ': start to change the joint motion {} with pose of [{}, {}]'.format(target_angles, target_pos, target_rot)) + ret = self.robot.setJointAngle(self.target_joint_names, target_angles, self.joint_thresh, self.timeout) + + if ret: + rospy.loginfo(self.__class__.__name__ + ': robot succeed to convernge to the {} th target joints {}:{}!'.format(i + 1, self.target_joint_names, target_angles)) + else: + rospy.logwarn(self.__class__.__name__ + ': timeout ({}sec), fail to reach the {} th target joints. preempted!'.format(self.timeout, i+1)) + return 'preempted' + + + # reset + timeout = start_time + self.timeout - rospy.get_time() + start_time = rospy.get_time() + + # check the pose convergence + ret = self.robot.poseConvergenceCheck(timeout, target_pos, target_rot, self.pos_thresh, rot_thresh = self.rot_thresh) + + if not ret: + rospy.logwarn(self.__class__.__name__ + ': timeout ({}sec), fail to reach the {} th target pose. preempted!'.format(i+1, self.timeout)) + return 'preempted' + + rospy.loginfo(self.__class__.__name__ + ': robot succeed to convernge to the {} th target pose {}: [{}, {}]!'.format(i + 1, self.target_joint_names, target_pos, target_rot)) + + self.hold(self.hold_time, userdata.flags) + + return 'succeeded' diff --git a/aerial_robot_base/src/hovering_check.py b/aerial_robot_base/src/hovering_check.py new file mode 100755 index 000000000..4ef524974 --- /dev/null +++ b/aerial_robot_base/src/hovering_check.py @@ -0,0 +1,112 @@ +#!/usr/bin/env python +# -*- coding: utf-8 -*- + +# Software License Agreement (BSD License) + +# Copyright (c) 2025, DRAGON 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. + +import sys +import unittest + +import rospy +import rostest +from aerial_robot_base.state_machine import * +from std_msgs.msg import Empty + + +PKG = 'rostest' + +# Template for simple demo +class HoverMotion(): + def __init__(self): + self.robot = RobotInterface() + + self.sm_top = smach.StateMachine(outcomes=['succeeded', 'preempted']) + self.sm_top.userdata.flags = {} + self.sm_top.userdata.extra = {} + + with self.sm_top: + smach.StateMachine.add('Start', Start(self.robot), + transitions={'succeeded':'Arm', + 'preempted':'preempted'}, + remapping={'flags':'flags', 'extra':'extra'}) + + + smach.StateMachine.add('Arm', Arm(self.robot), + transitions={'succeeded':'Takeoff', + 'preempted':'preempted'}, + remapping={'flags':'flags', 'extra':'extra'}) + + smach.StateMachine.add('Takeoff', Takeoff(self.robot), + transitions={'succeeded':'WayPoint', + 'preempted':'preempted'}, + remapping={'flags':'flags', 'extra':'extra'}) + + + smach.StateMachine.add('WayPoint', WayPoint(self.robot), + transitions={'succeeded':'Land', + 'preempted':'preempted'}, + remapping={'flags':'flags', 'extra':'extra'}) + + smach.StateMachine.add('Land', Land(self.robot), + transitions={'succeeded':'succeeded', + 'preempted':'preempted'}, + remapping={'flags':'flags', 'extra':'extra'}) + + self.sis = smach_ros.IntrospectionServer('task_smach_server', self.sm_top, '/SM_ROOT') + + + def startMotion(self): + self.sis.start() + outcome = self.sm_top.execute() + + self.sis.stop() + + if outcome == 'succeeded': + return True + else: + return False + + +class HoveringTest(unittest.TestCase): + def __init__(self, *args): + super(self.__class__, self).__init__(*args) + rospy.init_node("hovering_test") + + def test_hovering(self): + checker = HoverMotion() + self.assertTrue(checker.startMotion()) + +if __name__ == '__main__': + print("start check hovering") + try: + rostest.run(PKG, 'hovering_check', HoveringTest, sys.argv) + except KeyboardInterrupt: + pass + + print("exiting") diff --git a/robots/dragon/launch/bringup.launch b/robots/dragon/launch/bringup.launch index 1af4621dd..87a7cec3e 100644 --- a/robots/dragon/launch/bringup.launch +++ b/robots/dragon/launch/bringup.launch @@ -21,6 +21,7 @@ + ########### UAV Config ########### @@ -112,6 +113,9 @@ + ########## Simple Demo ######### + + diff --git a/robots/dragon/scripts/simple_demo.py b/robots/dragon/scripts/simple_demo.py new file mode 100755 index 000000000..f1710a26d --- /dev/null +++ b/robots/dragon/scripts/simple_demo.py @@ -0,0 +1,128 @@ +#!/usr/bin/env python +# -*- coding: utf-8 -*- + +# Software License Agreement (BSD License) + +# Copyright (c) 2025, DRAGON 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. + +import rospy +import math +import copy + +from aerial_robot_base.robot_interface import RobotInterface +from aerial_robot_base.state_machine import * + +# Simple Dragon demo +class DragonDemo(): + def __init__(self): + self.robot = RobotInterface() + rospy.sleep(1.0) # wait for joint updated + joint_names = self.robot.getJointState().name + joint_num = len(joint_names) + nominal_angle = 2 * math.pi / (joint_num / 2 + 1) + init_angles = [0, nominal_angle] * (joint_num // 2) + + # create a simple joint motion + end_angles = copy.deepcopy(init_angles) + end_angles[0] = 0 + joint_traj = [end_angles] + + # rotate cog + target_rot = [-0.3, 0.0] + rot_traj = [target_rot] + + # no position trajectory + pos_traj = [] + + # if it is quad type + if joint_num/2 == 3: + joint_traj = [[-0.6, -1.56, -0.6, 0, 0, 1.56], [0, 1.57, 0, 1.57, 0, 1.57], [0.0, 1.57, -1.5, 0.0, 0.0, 1.57]] + rot_traj = [[0.0, -0.3], [0, 0], [0, 0.75]] + + joint_thresh = 0.1 + pos_thresh = 0.1 + rot_thresh = 0.1 + + self.sm_top = smach.StateMachine(outcomes=['succeeded', 'preempted']) + self.sm_top.userdata.flags = {} + self.sm_top.userdata.extra = {} + + with self.sm_top: + smach.StateMachine.add('Start', Start(self.robot), + transitions={'succeeded':'FormCheck', + 'preempted':'preempted'}, + remapping={'flags':'flags', 'extra':'extra'}) + + + smach.StateMachine.add('FormCheck', FormCheck(self.robot, 'init_form', \ + joint_names, init_angles, joint_thresh), + transitions={'succeeded':'Arm', + 'preempted':'preempted'}, + remapping={'flags':'flags', 'extra':'extra'}) + + smach.StateMachine.add('Arm', Arm(self.robot), + transitions={'succeeded':'Takeoff', + 'preempted':'preempted'}, + remapping={'flags':'flags', 'extra':'extra'}) + + smach.StateMachine.add('Takeoff', Takeoff(self.robot), + transitions={'succeeded':'Transform', + 'preempted':'preempted'}, + remapping={'flags':'flags', 'extra':'extra'}) + + smach.StateMachine.add('Transform', TransformWithPose(self.robot, 'motion', joint_names, \ + joint_traj, pos_traj, rot_traj, \ + joint_thresh, pos_thresh, rot_thresh, \ + timeout = 20.0, hold_time = 2.0, \ + rotate_cog = True), + transitions={'succeeded':'Circle', + 'preempted':'preempted'}, + remapping={'flags':'flags', 'extra':'extra'}) + + smach.StateMachine.add('Circle', CircleTrajectory(self.robot, period = 6.0, radius = 0), + transitions={'succeeded':'Land', + 'preempted':'preempted'}, + remapping={'flags':'flags', 'extra':'extra'}) + + smach.StateMachine.add('Land', Land(self.robot), + transitions={'succeeded':'succeeded', + 'preempted':'preempted'}, + remapping={'flags':'flags', 'extra':'extra'}) + + self.sis = smach_ros.IntrospectionServer('task_smach_server', self.sm_top, '/SM_ROOT') + + + self.sis.start() + outcome = self.sm_top.execute() + + self.sis.stop() + +if __name__ == '__main__': + + rospy.init_node('dragon_demo') + demo = DragonDemo() diff --git a/robots/dragon/scripts/transformation_demo.py b/robots/dragon/scripts/transformation_demo.py deleted file mode 100755 index 8e5f9aca3..000000000 --- a/robots/dragon/scripts/transformation_demo.py +++ /dev/null @@ -1,51 +0,0 @@ -#!/usr/bin/env python - -import sys -import time -import rospy -import math -from sensor_msgs.msg import JointState -from spinal.msg import DesireCoord - -rospy.init_node("dragon_transformation_demo") - -joint_control_pub = rospy.Publisher("/dragon/joints_ctrl", JointState, queue_size=10) -att_control_pub = rospy.Publisher("/dragon/final_target_baselink_rot", DesireCoord, queue_size=1) - -demo_mode = rospy.get_param("~mode", 0) -reset = rospy.get_param("~reset", False) -reverse_reset = rospy.get_param("~reverse_reset", False) - -desire_joint = JointState() -desire_att = DesireCoord() - -half_pi = 1.56 # the limitation of the joint - -if demo_mode == 0: # sea horse - desire_joint.position = [-0.6, -half_pi, -0.6, 0, 0, half_pi] - desire_att.pitch = -0.3 -elif demo_mode == 1: # spiral model - desire_joint.position = [-0.8, 1.2, -0.8, 1.2, -0.8, 1.2] - desire_att.pitch = -0.4 - desire_att.roll = 0.4 -elif demo_mode == 2: # mode model - desire_joint.position = [0.0, half_pi, -1.5, 0.0, 0.0, half_pi] - desire_att.pitch = 0.75 -else: - sys.exit() - -if reset: - desire_joint.position = [0.0, half_pi, 0, half_pi, 0.0, half_pi] - desire_att.pitch = 0 - desire_att.roll = 0 - -if reverse_reset: - desire_joint.position = [0.0, -half_pi, 0, -half_pi, 0.0, -half_pi] - desire_att.pitch = 0 - desire_att.roll = 0 - -time.sleep(0.6) - -joint_control_pub.publish(desire_joint) -att_control_pub.publish(desire_att) - diff --git a/robots/dragon/scripts/transformation_demo_vertical.py b/robots/dragon/scripts/transformation_demo_vertical.py deleted file mode 100755 index c0d096a84..000000000 --- a/robots/dragon/scripts/transformation_demo_vertical.py +++ /dev/null @@ -1,42 +0,0 @@ -#!/usr/bin/env python - -import sys -import time -import rospy -import math -from sensor_msgs.msg import JointState -from spinal.msg import DesireCoord - -rospy.init_node("dragon_transformation_demo") - -joint_control_pub = rospy.Publisher("/dragon/joints_ctrl", JointState, queue_size=10) -att_control_pub = rospy.Publisher("/dragon/final_target_baselink_rot", DesireCoord, queue_size=1) - -demo_mode = rospy.get_param("~mode", 0) -#reset = rospy.get_param("~reset", False) -#reverse_reset = rospy.get_param("~reverse_reset", False) - -desire_joint = JointState() -desire_att = DesireCoord() - -half_pi = 1.56 # the limitation of the joint - -if demo_mode == 0: - desire_joint.name = ["joint3_yaw"] - desire_joint.position = [-half_pi] - -elif demo_mode == 1: - desire_joint.name = ["joint2_yaw"] - desire_joint.position = [0] - -elif demo_mode == 2: # mode model - desire_joint.name = ["joint2_yaw", "joint1_yaw"] - desire_joint.position = [-half_pi, -half_pi] -else: - sys.exit() - -time.sleep(0.6) - -joint_control_pub.publish(desire_joint) - - diff --git a/robots/dragon/test/dragon_control.test b/robots/dragon/test/dragon_control.test index e22d7d8af..b0ad2262f 100644 --- a/robots/dragon/test/dragon_control.test +++ b/robots/dragon/test/dragon_control.test @@ -3,6 +3,7 @@ + @@ -10,81 +11,59 @@ + + - - + - topics: - - name: uav/cog/odom - timeout: 5.0 + init_form: + joint_names: ['joint1_pitch', 'joint1_yaw', 'joint2_pitch', 'joint2_yaw', 'joint3_pitch', 'joint3_yaw'] + joint_angles: [0, 1.57, 0, 1.57, 0, 1.57] + angle_thresh: 0.08 + timeout: 20.0 + takeoff: + timeout: 30.0 + hold_time: 2.0 + motion: + joint_names: ['joint1_pitch', 'joint1_yaw', 'joint2_pitch', 'joint2_yaw', 'joint3_pitch', 'joint3_yaw'] + joint_trajectory: [[-0.6, -1.57, -0.6, 0, 0, 1.57], [0, 1.57, 0, 1.57, 0, 1.57], [-0.8, 1.2, -0.8, 1.2, -0.8, 1.2], [0, 1.57, 0, 1.57, 0, 1.57], [0.0, 1.57, -1.5, 0.0, 0.0, 1.57]] + rot_trajectory: [[0.0, -0.3], [0, 0], [-0.4, 0.4], [0, 0], [0, 0.75]] + joint_thresh: 0.04 + pos_thresh: [0.05, 0.05, 0.05] + rot_thresh: [0.05, 0.05, 0.05] + timeout: 20.0 + hold_time: 2.0 + stable_check: + pose_thresh: [0.1, 0.15, 0.1] - - + - hovering_duration: 30.0 - convergence_thresholds: [0.02, 0.02, 0.02] # hovering threshold - init_joint_names: ['joint1_pitch', 'joint1_yaw', 'joint2_pitch', 'joint2_yaw', 'joint3_pitch', 'joint3_yaw'] - init_joint_angles: [0, 1.57, 0, 1.57, 0, 1.57] - tasks: - - command: "rosrun dragon transformation_demo.py _mode:=0 _reset:=0" - threshold: [0.1, 0.08, 0.08] - angle_threshold: 0.02 - timeout: 20 - reset: "rosrun dragon transformation_demo.py _mode:=0 _reset:=1" - reset_duration: 20 - - command: "rosrun dragon transformation_demo.py _mode:=1 _reset:=0" - threshold: [0.08, 0.08, 0.05] - angle_threshold: 0.02 - timeout: 10 - reset: "rosrun dragon transformation_demo.py _mode:=0 _reset:=1" - reset_duration: 10 - - command: "rosrun dragon transformation_demo.py _mode:=2 _reset:=0" - threshold: [0.15, 0.08, 0.12] - angle_threshold: 0.02 - timeout: 15 - reset: "rosrun dragon transformation_demo.py _mode:=0 _reset:=1" - reset_duration: 10 + init_form: + joint_names: ['joint1_pitch', 'joint1_yaw', 'joint2_pitch', 'joint2_yaw', 'joint3_pitch', 'joint3_yaw'] + joint_angles: [0, 1.57, 0, 1.57, 0, 1.57] + angle_thresh: 0.08 + timeout: 20.0 + takeoff: + timeout: 30.0 + hold_time: 2.0 + motion: + joint_names: ['joint1_pitch', 'joint1_yaw', 'joint2_pitch', 'joint2_yaw', 'joint3_pitch', 'joint3_yaw'] + joint_trajectory: [[0, 1.57, 0, 1.57, 0, 1.57], [0, 1.57, 0, 1.57, 0, -1.57], [0, 1.57, 0, 0, 0, -1.57]] + rot_trajectory: [[-1.5708, 0], [-1.5708, 0], [-1.5708, 0]] + joint_thresh: 0.04 + pos_thresh: [0.05, 0.05, 0.05] + rot_thresh: [0.05, 0.05, 0.05] + timeout: 20.0 + hold_time: 2.0 + stable_check: + pose_thresh: [0.1, 0.1, 0.1] - - - - - hovering_duration: 30.0 - convergence_thresholds: [0.02, 0.02, 0.02] # hovering threshold - init_joint_names: ['joint1_pitch', 'joint1_yaw', 'joint2_pitch', 'joint2_yaw', 'joint3_pitch', 'joint3_yaw'] - init_joint_angles: [0, 1.57, 0, 1.57, 0, 1.57] - tasks: - - command: "rostopic pub -1 /dragon/final_target_baselink_rot spinal/DesireCoord -- -1.5708 0.0 0.0 0" - threshold: [0.06, 0.06, 0.06] - angle_threshold: 0.02 - timeout: 15 - reset_duration: 0 - - command: "rosrun dragon transformation_demo_vertical.py _mode:=0" - threshold: [0.08, 0.08, 0.06] - angle_threshold: 0.02 - timeout: 20 - reset_duration: 0 - - command: "rosrun dragon transformation_demo_vertical.py _mode:=1" - threshold: [0.08, 0.08, 0.06] - angle_threshold: 0.02 - timeout: 20 - reset_duration: 0 - - - - - + diff --git a/robots/dragon/test/flight_check.py b/robots/dragon/test/flight_check.py new file mode 100755 index 000000000..b5ef55d08 --- /dev/null +++ b/robots/dragon/test/flight_check.py @@ -0,0 +1,164 @@ +#!/usr/bin/env python +# -*- coding: utf-8 -*- + +# Software License Agreement (BSD License) + +# Copyright (c) 2025, DRAGON 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. + +import sys +import unittest +import math + +import rospy +import rostest +from aerial_robot_base.state_machine import * +from aerial_robot_msgs.msg import PoseControlPid + +PKG = 'rostest' + +class FlightMotion(): + def __init__(self): + self.robot = RobotInterface() + + self.control_term_sub = rospy.Subscriber('debug/pose/pid', PoseControlPid, self.controlTermCallback) + self.divergent_flag = False + self.pose_tresh = rospy.get_param('~stable_check/pose_thresh', [0.05, 0.05, 0.05]) # xy, z, yaw + self.max_err_xy = 0 + self.max_err_x = 0 + self.max_err_y = 0 + self.max_err_z = 0 + self.max_err_yaw = 0 + + joint_names = self.robot.getJointState().name + + self.sm_top = smach.StateMachine(outcomes=['succeeded', 'preempted']) + self.sm_top.userdata.flags = {} + self.sm_top.userdata.extra = {} + + with self.sm_top: + smach.StateMachine.add('Start', Start(self.robot), + transitions={'succeeded':'FormCheck', + 'preempted':'preempted'}, + remapping={'flags':'flags', 'extra':'extra'}) + + + smach.StateMachine.add('FormCheck', FormCheck(self.robot, 'init_form'), + transitions={'succeeded':'Arm', + 'preempted':'preempted'}, + remapping={'flags':'flags', 'extra':'extra'}) + + smach.StateMachine.add('Arm', Arm(self.robot), + transitions={'succeeded':'Takeoff', + 'preempted':'preempted'}, + remapping={'flags':'flags', 'extra':'extra'}) + + smach.StateMachine.add('Takeoff', Takeoff(self.robot), + transitions={'succeeded':'Motion', + 'preempted':'preempted'}, + remapping={'flags':'flags', 'extra':'extra'}) + + smach.StateMachine.add('Motion', TransformWithPose(self.robot, rotate_cog = True), + transitions={'succeeded':'Land', + 'preempted':'preempted'}, + remapping={'flags':'flags', 'extra':'extra'}) + + smach.StateMachine.add('Land', Land(self.robot), + transitions={'succeeded':'succeeded', + 'preempted':'preempted'}, + remapping={'flags':'flags', 'extra':'extra'}) + + self.sis = smach_ros.IntrospectionServer('task_smach_server', self.sm_top, '/SM_ROOT') + + + def startMotion(self): + self.sis.start() + outcome = self.sm_top.execute() + + self.sis.stop() + + rospy.loginfo("max pose errors in hovering is [xy, z, yaw] are [%f (%f, %f), %f, %f]", self.max_err_xy, self.max_err_x, self.max_err_y, self.max_err_z, self.max_err_yaw) + if self.divergent_flag: + rospy.logerr("because of devergence, the test is filaed") + outcome = 'premmpted' + + if outcome == 'succeeded': + return True + else: + return False + + + def controlTermCallback(self, msg): + + if self.robot.getFlightState() != self.robot.HOVER_STATE: + return + + if self.divergent_flag: + return + + err_x = msg.x.err_p + err_y = msg.y.err_p + err_z = msg.z.err_p + err_yaw = msg.yaw.err_p + err_xy = math.sqrt(err_x * err_x + err_y * err_y) + + if err_xy > self.max_err_xy: + self.max_err_xy = err_xy + self.max_err_x = err_x + self.max_err_y = err_y + if math.fabs(err_z) > math.fabs(self.max_err_z): + self.max_err_z = err_z + if math.fabs(err_yaw) > math.fabs(self.max_err_yaw): + self.max_err_yaw = err_yaw + + # check the control stability + if err_xy > self.pose_tresh[0] or math.fabs(err_z) > self.pose_tresh[1] or math.fabs(err_yaw) > self.pose_tresh[2]: + rospy.logerr("devergence in [xy, z, yaw]: [%f (%f, %f), %f, %f]", err_xy, err_x, err_y, err_z, err_yaw) + self.divergent_flag = True + + +class FlightTest(unittest.TestCase): + def __init__(self, *args): + super(self.__class__, self).__init__(*args) + rospy.init_node("flight_test") + + def test_flight(self): + checker = FlightMotion() + try: + self.assertTrue(checker.startMotion()) + except ValueError as e: + print("e") + self.assertTrue(False) + +if __name__ == '__main__': + print("start check flight") + try: + rostest.run(PKG, 'flight_check', FlightTest, sys.argv) + except KeyboardInterrupt: + pass + + print("exiting") diff --git a/robots/hydrus/launch/bringup.launch b/robots/hydrus/launch/bringup.launch index 2a15608d8..039f6be7a 100644 --- a/robots/hydrus/launch/bringup.launch +++ b/robots/hydrus/launch/bringup.launch @@ -20,6 +20,7 @@ + ########### Parameters ########### @@ -107,4 +108,7 @@ + ########## Simple Demo ######### + + diff --git a/robots/hydrus/scripts/hydrus_demo.py b/robots/hydrus/scripts/hydrus_demo.py deleted file mode 100755 index 84bc7fe6c..000000000 --- a/robots/hydrus/scripts/hydrus_demo.py +++ /dev/null @@ -1,29 +0,0 @@ -#!/usr/bin/env python - -import sys -import time -import rospy -import math -from sensor_msgs.msg import JointState - -if __name__ == "__main__": - - rospy.init_node("hydrus_demo") - - link_num = rospy.get_param("~link_num", 4) - duration = rospy.get_param("~duration", 8) - joint_control_topic_name = rospy.get_param("~joint_control_topic_name", "joints_ctrl") - pub = rospy.Publisher(joint_control_topic_name, JointState, queue_size=10) - - time.sleep(1) - joint = JointState() - joint.position = [] - for i in range(0, link_num - 1): - joint.position.append(2 * math.pi / link_num) - while not rospy.is_shutdown(): - - for i in range(0, link_num - 1): - joint.position[i] = -joint.position[i] - pub.publish(joint) - print("joint angles: {}".format(joint.position)) - time.sleep(duration) diff --git a/robots/hydrus/scripts/loop_demo.py b/robots/hydrus/scripts/loop_demo.py new file mode 100755 index 000000000..178d42be2 --- /dev/null +++ b/robots/hydrus/scripts/loop_demo.py @@ -0,0 +1,115 @@ +#!/usr/bin/env python +# -*- coding: utf-8 -*- + +# Software License Agreement (BSD License) + +# Copyright (c) 2025, DRAGON 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. + +import rospy +import math +import copy + +from aerial_robot_base.robot_interface import RobotInterface +from aerial_robot_base.state_machine import * + + +# Simple Hydrus demo +class HydrusDemo(): + def __init__(self): + self.robot = RobotInterface() + rospy.sleep(1.0) # wait for joint updated + joint_names = self.robot.getJointState().name + joint_num = len(joint_names) + nominal_angle = 2 * math.pi / (joint_num + 1) + init_angles = [nominal_angle] * joint_num + trajectory = [init_angles] + + self.sm_top = smach.StateMachine(outcomes=['succeeded', 'preempted']) + self.sm_top.userdata.flags = {} + self.sm_top.userdata.extra = {} + + with self.sm_top: + smach.StateMachine.add('Start', Start(self.robot), + transitions={'succeeded':'FormCheck', + 'preempted':'preempted'}, + remapping={'flags':'flags', 'extra':'extra'}) + + + smach.StateMachine.add('FormCheck', FormCheck(self.robot, 'init_form', \ + joint_names = joint_names, \ + joint_angles = init_angles, \ + thresh = 0.05), + transitions={'succeeded':'Arm', + 'preempted':'preempted'}, + remapping={'flags':'flags', 'extra':'extra'}) + + smach.StateMachine.add('Arm', Arm(self.robot), + transitions={'succeeded':'Takeoff', + 'preempted':'preempted'}, + remapping={'flags':'flags', 'extra':'extra'}) + + smach.StateMachine.add('Takeoff', Takeoff(self.robot), + transitions={'succeeded':'Transform', + 'preempted':'preempted'}, + remapping={'flags':'flags', 'extra':'extra'}) + + # create a loop motion + loop = rospy.get_param('~loop', 1) + for i in range(loop): + for j in range(2 * joint_num): + j = j % joint_num + angles = copy.deepcopy(trajectory[-1]) + angles[j] *= -1 + trajectory.append(angles) + + smach.StateMachine.add('Transform', Transform(self.robot, 'transfrom', \ + joint_names, trajectory, \ + thresh = 0.05, timeout = 20.0, \ + hold_time = 0.0), + transitions={'succeeded':'Land', + 'preempted':'preempted'}, + remapping={'flags':'flags', 'extra':'extra'}) + + smach.StateMachine.add('Land', Land(self.robot), + transitions={'succeeded':'succeeded', + 'preempted':'preempted'}, + remapping={'flags':'flags', 'extra':'extra'}) + + self.sis = smach_ros.IntrospectionServer('task_smach_server', self.sm_top, '/SM_ROOT') + + self.sis.start() + outcome = self.sm_top.execute() + + self.sis.stop() + + +if __name__ == '__main__': + + rospy.init_node('hydrus_demo') + + demo = HydrusDemo() diff --git a/robots/hydrus/scripts/simple_demo.py b/robots/hydrus/scripts/simple_demo.py new file mode 100755 index 000000000..8e34a74f1 --- /dev/null +++ b/robots/hydrus/scripts/simple_demo.py @@ -0,0 +1,109 @@ +#!/usr/bin/env python +# -*- coding: utf-8 -*- + +# Software License Agreement (BSD License) + +# Copyright (c) 2025, DRAGON 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. + +import rospy +import math +import copy + +from aerial_robot_base.robot_interface import RobotInterface +from aerial_robot_base.state_machine import * + +# Simple Hydrus demo +class HydrusDemo(): + def __init__(self): + self.robot = RobotInterface() + rospy.sleep(1.0) # wait for joint updated + joint_names = self.robot.getJointState().name + joint_num = len(joint_names) + nominal_angle = 2 * math.pi / (joint_num + 1) + init_angles = [nominal_angle] * joint_num + + # create a simple joint motion + end_angles = copy.deepcopy(init_angles) + end_angles[0] = 0 + trajectory = [end_angles] + + trajectory = rospy.get_param('~trajectory', trajectory) + + + self.sm_top = smach.StateMachine(outcomes=['succeeded', 'preempted']) + self.sm_top.userdata.flags = {} + self.sm_top.userdata.extra = {} + + with self.sm_top: + smach.StateMachine.add('Start', Start(self.robot), + transitions={'succeeded':'FormCheck', + 'preempted':'preempted'}, + remapping={'flags':'flags', 'extra':'extra'}) + + + smach.StateMachine.add('FormCheck', FormCheck(self.robot, 'init_form', joint_names, init_angles, thresh = 0.05), + transitions={'succeeded':'Arm', + 'preempted':'preempted'}, + remapping={'flags':'flags', 'extra':'extra'}) + + smach.StateMachine.add('Arm', Arm(self.robot), + transitions={'succeeded':'Takeoff', + 'preempted':'preempted'}, + remapping={'flags':'flags', 'extra':'extra'}) + + smach.StateMachine.add('Takeoff', Takeoff(self.robot), + transitions={'succeeded':'Transform', + 'preempted':'preempted'}, + remapping={'flags':'flags', 'extra':'extra'}) + + smach.StateMachine.add('Transform', Transform(self.robot, 'transfrom', \ + joint_names, trajectory, \ + thresh = 0.05, timeout = 20.0, \ + hold_time = 0.0), + transitions={'succeeded':'Land', + 'preempted':'preempted'}, + remapping={'flags':'flags', 'extra':'extra'}) + + smach.StateMachine.add('Land', Land(self.robot), + transitions={'succeeded':'succeeded', + 'preempted':'preempted'}, + remapping={'flags':'flags', 'extra':'extra'}) + + self.sis = smach_ros.IntrospectionServer('task_smach_server', self.sm_top, '/SM_ROOT') + + + self.sis.start() + outcome = self.sm_top.execute() + + self.sis.stop() + +if __name__ == '__main__': + + rospy.init_node('hydrus_demo') + + demo = HydrusDemo() diff --git a/robots/hydrus/test/control_check.py b/robots/hydrus/test/control_check.py deleted file mode 100755 index 9d07c3311..000000000 --- a/robots/hydrus/test/control_check.py +++ /dev/null @@ -1,245 +0,0 @@ -#!/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. - -import os -import sys -import rospy -import unittest -import rostest -import operator -from functools import reduce -import shlex -import subprocess -import math -from aerial_robot_base.hovering_check import HoveringCheck -from sensor_msgs.msg import JointState - -PKG = 'rostest' - -class InitFormCheck(): - def __init__(self, name="init_form_check"): - self.init_joint_names = rospy.get_param('~init_joint_names', []) - self.init_joint_angles = rospy.get_param('~init_joint_angles', []) - self.convergence_duration = rospy.get_param('~init_form_duration', 10.0) - self.angle_threshold = rospy.get_param('~init_angle_threshold', 0.02) # rad - - self.joint_angle_sub = rospy.Subscriber('joint_states', JointState, self._jointCallback) - self.joint_msg = None - self.joint_map = [] - - def initFormCheck(self): - deadline = rospy.Time.now() + rospy.Duration(self.convergence_duration) - while not rospy.Time.now() > deadline: - if self.joint_msg is not None: - joint_angles = [self.joint_msg.position[i] for i in self.joint_map] - rospy.loginfo_throttle(1, '%s: %s' % (self.init_joint_names, joint_angles)) - - rospy.sleep(1.0) - - # check convergence - joint_angles = [self.joint_msg.position[i] for i in self.joint_map] - - angles_diff = [math.fabs(x - y) < self.angle_threshold for (x, y) in zip(self.init_joint_angles, joint_angles)] - rospy.loginfo('joint angles convergence: %s: %s' % (self.init_joint_names, angles_diff)) - if reduce(operator.mul, angles_diff): - return True - else: - # cannot be convergent - return False - - def _jointCallback(self, msg): - if self.joint_msg == None: - self.joint_map = [msg.name.index(i) for i in self.init_joint_names] - - self.joint_msg = msg - -class TransformCheck(InitFormCheck, HoveringCheck): - def __init__(self, name="init_form_check"): - InitFormCheck.__init__(self) - HoveringCheck.__init__(self) - - # list of the transform task - self.params = rospy.get_param('~tasks', []) - - self.joint_ctrl_sub = rospy.Subscriber('joints_ctrl', JointState, self._jointCtrlCallback) - self.joint_ctrl_pub = rospy.Publisher('joints_ctrl', JointState, queue_size=1) - - self.target_joint_angles = self.init_joint_angles - - def transformCheck(self): - rospy.sleep(0.5) - - for param in self.params: - index = self.params.index(param) + 1 - print("start task%d" % index) - task = {'timeout': 10.0, 'threshold': [0.01, 0.01, 0.01], 'angle_threshold': 0.01, 'reset': None, 'reset_duration': 10.0} - - task.update(param) - - print(task) - - deadline = rospy.Time.now() + rospy.Duration(task['timeout']) - - node_pid = None - if isinstance(task['command'], list): - print("joint ctrl") - # the target joint angles - assert len(task['command']) == len(self.init_joint_angles), 'the length of target joint angles from init_joint_anlges is wrong' - - self.target_joint_angles = task['command'] - joint_ctrl_msg = JointState() - joint_ctrl_msg.header.stamp = rospy.Time.now() - joint_ctrl_msg.position = self.target_joint_angles - self.joint_ctrl_pub.publish(joint_ctrl_msg) - else: - print("string command") - # the rosrun command (TODO, the roslaunch) - node_command = shlex.split(task['command']) - assert node_command[0] == 'rosrun' or node_command[0] == 'rostopic', 'please use rosrun command' - # start the rosnode - node_pid = subprocess.Popen(node_command) - - max_error_xy = 0 - max_error_x = 0 - max_error_y = 0 - max_error_z = 0 - max_error_yaw = 0 - while not rospy.Time.now() > deadline: - rospy.sleep(0.1) - - if self.joint_msg is None or self.control_msg is None: - continue - - 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) - - joint_angles = [self.joint_msg.position[i] for i in self.joint_map] - rospy.loginfo_throttle(1, "errors in [xy, z, yaw]: [{} ({}, {}), {}, {}]; joint angles: {}".format(err_xy, err_x, err_y, err_z, err_yaw, joint_angles)) - - if err_xy > max_error_xy: - max_error_xy = err_xy - max_error_x = err_x - max_error_y = err_y - if math.fabs(err_z) > math.fabs(max_error_z): - max_error_z = err_z - if math.fabs(err_yaw) > math.fabs(max_error_yaw): - max_error_yaw = err_yaw - - # check the control stability - if err_xy > task['threshold'][0] or math.fabs(err_z) > task['threshold'][1] or math.fabs(err_yaw) > task['threshold'][2]: - rospy.logwarn("devergence in [xy, z, yaw]: [%f (%f, %f), %f, %f]", err_xy, err_x, err_y, err_z, err_yaw) - if node_pid: - node_pid.kill() - return False - - if node_pid: - node_pid.kill() - - rospy.loginfo("max errors in [xy, z, yaw] are [%f (%f, %f), %f, %f]", max_error_xy, max_error_x, max_error_y, max_error_z, max_error_yaw) - - # check joint angles convergence - joint_angles = [self.joint_msg.position[i] for i in self.joint_map] - angles_diff = [math.fabs(x - y) < task['angle_threshold'] for (x, y) in zip(self.target_joint_angles, joint_angles)] - if not reduce(operator.mul, angles_diff): - rospy.logwarn('angles diff is devergent: %s: %s' % (self.init_joint_names, [(x - y) for (x,y) in zip(self.target_joint_angles, joint_angles)])) - # cannot be convergent - return False - - node_pid = None - # reset the form - if isinstance(task['reset'], list): - # the target joint angles - assert len(task['reset']) == len(self.init_joint_angles), 'the length of target joint angles from init_joint_anlges is wrong' - - self.target_joint_angles = task['reset'] - joint_ctrl_msg = JointState() - joint_ctrl_msg.header.stamp = rospy.Time.now() - joint_ctrl_msg.position = self.target_joint_angles - self.joint_ctrl_pub.publish(joint_ctrl_msg) - - rospy.sleep(task['reset_duration']) - rospy.loginfo("reset the form") - - elif isinstance(task['reset'], str): - # the rosrun command (TODO, the roslaunch) - node_command = shlex.split(task['reset']) - assert node_command[0] == 'rosrun', 'please use rosrun command' - # start the rosnode - node_pid = subprocess.Popen(node_command) - - rospy.sleep(task['reset_duration']) - rospy.loginfo("reset the form") - - node_pid.kill() - - return True - - def _jointCtrlCallback(self, msg): - if len(msg.position) == len(self.target_joint_angles): - self.target_joint_angles = msg.position - else: - for i, postion in enumerate(msg.position): - index = self.init_joint_names.index(msg.name[i]) - self.target_joint_angles[index] = msg.position[i] - - -class ControlTest(unittest.TestCase): - def __init__(self, *args): - super(self.__class__, self).__init__(*args) - rospy.init_node("control_test") - - def test_control(self): - # step1: check the init form convergence - init_form_checker = InitFormCheck() - self.assertTrue(init_form_checker.initFormCheck(), msg = 'Cannot reach convergence for initial form') - - # steup2: check hovering - hovering_checker = HoveringCheck() - self.assertTrue(hovering_checker.hoveringCheck(), msg = 'Cannot reach convergence for hovering') - - # steup3: check transformation - transform_checker = TransformCheck() - self.assertTrue(transform_checker.transformCheck(), msg = 'Cannot reach convergence for transforming hovering') - - -if __name__ == '__main__': - print("start check flight control for hydrus") - try: - rostest.run(PKG, 'control_check', ControlTest, sys.argv) - except KeyboardInterrupt: - pass - - print("exiting") - diff --git a/robots/hydrus/test/flight_check.py b/robots/hydrus/test/flight_check.py new file mode 100755 index 000000000..9716fbb4e --- /dev/null +++ b/robots/hydrus/test/flight_check.py @@ -0,0 +1,164 @@ +#!/usr/bin/env python +# -*- coding: utf-8 -*- + +# Software License Agreement (BSD License) + +# Copyright (c) 2025, DRAGON 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. + +import sys +import unittest +import math + +import rospy +import rostest +from aerial_robot_base.state_machine import * +from aerial_robot_msgs.msg import PoseControlPid + +PKG = 'rostest' + +class FlightMotion(): + def __init__(self): + self.robot = RobotInterface() + + self.control_term_sub = rospy.Subscriber('debug/pose/pid', PoseControlPid, self.controlTermCallback) + self.divergent_flag = False + self.pose_tresh = rospy.get_param('~stable_check/pose_thresh', [0.05, 0.05, 0.05]) # xy, z, yaw + self.max_err_xy = 0 + self.max_err_x = 0 + self.max_err_y = 0 + self.max_err_z = 0 + self.max_err_yaw = 0 + + joint_names = self.robot.getJointState().name + + self.sm_top = smach.StateMachine(outcomes=['succeeded', 'preempted']) + self.sm_top.userdata.flags = {} + self.sm_top.userdata.extra = {} + + with self.sm_top: + smach.StateMachine.add('Start', Start(self.robot), + transitions={'succeeded':'FormCheck', + 'preempted':'preempted'}, + remapping={'flags':'flags', 'extra':'extra'}) + + + smach.StateMachine.add('FormCheck', FormCheck(self.robot, 'init_form'), + transitions={'succeeded':'Arm', + 'preempted':'preempted'}, + remapping={'flags':'flags', 'extra':'extra'}) + + smach.StateMachine.add('Arm', Arm(self.robot), + transitions={'succeeded':'Takeoff', + 'preempted':'preempted'}, + remapping={'flags':'flags', 'extra':'extra'}) + + smach.StateMachine.add('Takeoff', Takeoff(self.robot), + transitions={'succeeded':'Motion', + 'preempted':'preempted'}, + remapping={'flags':'flags', 'extra':'extra'}) + + smach.StateMachine.add('Motion', Transform(self.robot), + transitions={'succeeded':'Land', + 'preempted':'preempted'}, + remapping={'flags':'flags', 'extra':'extra'}) + + smach.StateMachine.add('Land', Land(self.robot), + transitions={'succeeded':'succeeded', + 'preempted':'preempted'}, + remapping={'flags':'flags', 'extra':'extra'}) + + self.sis = smach_ros.IntrospectionServer('task_smach_server', self.sm_top, '/SM_ROOT') + + + def startMotion(self): + self.sis.start() + outcome = self.sm_top.execute() + + self.sis.stop() + + rospy.loginfo("max pose errors in hovering is [xy, z, yaw] are [%f (%f, %f), %f, %f]", self.max_err_xy, self.max_err_x, self.max_err_y, self.max_err_z, self.max_err_yaw) + if self.divergent_flag: + rospy.logerr("because of devergence, the test is filaed") + outcome = 'premmpted' + + if outcome == 'succeeded': + return True + else: + return False + + + def controlTermCallback(self, msg): + + if self.robot.getFlightState() != self.robot.HOVER_STATE: + return + + if self.divergent_flag: + return + + err_x = msg.x.err_p + err_y = msg.y.err_p + err_z = msg.z.err_p + err_yaw = msg.yaw.err_p + err_xy = math.sqrt(err_x * err_x + err_y * err_y) + + if err_xy > self.max_err_xy: + self.max_err_xy = err_xy + self.max_err_x = err_x + self.max_err_y = err_y + if math.fabs(err_z) > math.fabs(self.max_err_z): + self.max_err_z = err_z + if math.fabs(err_yaw) > math.fabs(self.max_err_yaw): + self.max_err_yaw = err_yaw + + # check the control stability + if err_xy > self.pose_tresh[0] or math.fabs(err_z) > self.pose_tresh[1] or math.fabs(err_yaw) > self.pose_tresh[2]: + rospy.logerr("devergence in [xy, z, yaw]: [%f (%f, %f), %f, %f]", err_xy, err_x, err_y, err_z, err_yaw) + self.divergent_flag = True + + +class FlightTest(unittest.TestCase): + def __init__(self, *args): + super(self.__class__, self).__init__(*args) + rospy.init_node("flight_test") + + def test_flight(self): + checker = FlightMotion() + try: + self.assertTrue(checker.startMotion()) + except ValueError as e: + print("e") + self.assertTrue(False) + +if __name__ == '__main__': + print("start check flight") + try: + rostest.run(PKG, 'flight_check', FlightTest, sys.argv) + except KeyboardInterrupt: + pass + + print("exiting") diff --git a/robots/hydrus/test/hydrus_control.test b/robots/hydrus/test/hydrus_control.test index 1e039a3a0..35b3714b2 100644 --- a/robots/hydrus/test/hydrus_control.test +++ b/robots/hydrus/test/hydrus_control.test @@ -2,7 +2,8 @@ - + + @@ -10,38 +11,32 @@ + + - - + - topics: - - name: uav/cog/odom - timeout: 5.0 + init_form: + joint_names: ['joint1', 'joint2', 'joint3'] + joint_angles: [1.57, 1.57, 1.57] + angle_thresh: 0.05 + timeout: 20.0 + takeoff: + timeout: 30.0 + hold_time: 2.0 + transform: + joint_names: ['joint1', 'joint2', 'joint3'] + joint_trajectory: [[0, 0, 1.57], [1.57, 1.57, 1.57], [-1.57, 1.57, 1.57], [-1.57, -1.57, 1.57], [-1.57, -1.57, -1.57], [1.57, -1.57, -1.57], [1.57, 1.57, -1.57], [1.57, 1.57, 1.57]] + angle_thresh: 0.05 + timeout: 20.0 + hold_time: 2.0 + stable_check: + pose_thresh: [0.3, 0.05, 0.45] - - - - init_joint_names: ['joint1', 'joint2', 'joint3'] - init_joint_angles: [1.57, 1.57, 1.57] - tasks: - - command: [1.57, 0, 0] - threshold: [0.35, 0.01, 0.4] - reset: [1.57, 1.57, 1.57] - timeout: 20 - - command: [0, 0, 1.57] - threshold: [0.25, 0.01, 0.15] - timeout: 10 - - command: "rosrun hydrus hydrus_demo.py _link_num:=4 _duration:=6" - threshold: [0.3, 0.02, 0.45] - angle_threshold: 3.14 - timeout: 40 - - - - + diff --git a/robots/hydrus/test/tilted_hydrus_control.test b/robots/hydrus/test/tilted_hydrus_control.test index b6e11aea3..0dc53d424 100644 --- a/robots/hydrus/test/tilted_hydrus_control.test +++ b/robots/hydrus/test/tilted_hydrus_control.test @@ -10,38 +10,31 @@ + - - + - topics: - - name: uav/cog/odom - timeout: 5.0 + init_form: + joint_names: ['joint1', 'joint2', 'joint3'] + joint_angles: [1.57, 1.57, 1.57] + angle_thresh: 0.05 + timeout: 20.0 + takeoff: + timeout: 30.0 + hold_time: 2.0 + transform: + joint_names: ['joint1', 'joint2', 'joint3'] + joint_trajectory: [[0, 1.57, 1.57], [1.57, 1.57, 1.57], [0.78, 1.57, 0.78]] + angle_thresh: 0.05 + timeout: 20.0 + hold_time: 2.0 + stable_check: + pose_thresh: [0.05, 0.05, 0.06] - - - - init_joint_names: ['joint1', 'joint2', 'joint3'] - init_joint_angles: [1.57, 1.57, 1.57] - tasks: - - command: [0, 1.57, 1.57] - threshold: [0.05, 0.05, 0.06] - angle_threshold: 0.025 - reset: [1.57, 1.57, 1.57] - timeout: 10 - reset_duration: 5 - - command: [0.78, 1.57, 0.78] - threshold: [0.03, 0.02, 0.02] - angle_threshold: 0.025 - timeout: 10 - reset_duration: 5 - - - - + diff --git a/robots/hydrus_xi/launch/bringup.launch b/robots/hydrus_xi/launch/bringup.launch index 3b7d4e255..62e0864a9 100644 --- a/robots/hydrus_xi/launch/bringup.launch +++ b/robots/hydrus_xi/launch/bringup.launch @@ -24,6 +24,7 @@ + ########### Parameters ########### @@ -112,4 +113,7 @@ + ########## Simple Demo ######### + + diff --git a/robots/hydrus_xi/test/hydrus_xi_hex_branch_control.test b/robots/hydrus_xi/test/hydrus_xi_hex_branch_control.test index 79c5d5213..f87b2fbce 100644 --- a/robots/hydrus_xi/test/hydrus_xi_hex_branch_control.test +++ b/robots/hydrus_xi/test/hydrus_xi_hex_branch_control.test @@ -9,28 +9,20 @@ + - - + - topics: - - name: uav/cog/odom - timeout: 5.0 + takeoff: + timeout: 30.0 + waypoint: + waypoints: [[0, 0, 1.6]] + pos_thresh: 0.02 + yaw_thresh: 0.02 - - - - - init_joint_names: ['joint1', 'joint2', 'joint3', 'joint4', 'joint5', 'joint6'] - init_joint_angles: [0, 0, 0, 0, 0, 0] - convergence_thresholds: [0.01, 0.02, 0.01] - - - - - + diff --git a/robots/mini_quadrotor/launch/bringup.launch b/robots/mini_quadrotor/launch/bringup.launch index e262ec421..0d8fa925f 100644 --- a/robots/mini_quadrotor/launch/bringup.launch +++ b/robots/mini_quadrotor/launch/bringup.launch @@ -20,6 +20,7 @@ + ########### Parameters ########### @@ -98,4 +99,8 @@ + ########## Simple Demo ######### + + + diff --git a/robots/mini_quadrotor/test/flight_control.test b/robots/mini_quadrotor/test/flight_control.test index 9bc4a6c76..f6686594f 100644 --- a/robots/mini_quadrotor/test/flight_control.test +++ b/robots/mini_quadrotor/test/flight_control.test @@ -10,25 +10,17 @@ + - - - - topics: - - name: uav/cog/odom - timeout: 5.0 - - - - - hovering_delay: 5.0 - hovering_duration: 10.0 - convergence_thresholds: [0.02, 0.02, 0.02] # hovering threshold + waypoint: + waypoints: [[0, 0, 1.6]] + +