From fed257fdc2ef6cedbc6fe98a7a15ff472700147c Mon Sep 17 00:00:00 2001 From: Moju Zhao Date: Sun, 12 Jan 2025 12:23:18 +0900 Subject: [PATCH 1/9] [RosTest] move the module of hovering check to "src" directory --- .github/workflows/catkin_lint.yml | 2 +- aerial_robot_base/setup.py | 2 +- aerial_robot_base/{test => src}/aerial_robot_base/__init__.py | 0 .../{test => src}/aerial_robot_base/hovering_check.py | 0 4 files changed, 2 insertions(+), 2 deletions(-) rename aerial_robot_base/{test => src}/aerial_robot_base/__init__.py (100%) rename aerial_robot_base/{test => src}/aerial_robot_base/hovering_check.py (100%) diff --git a/.github/workflows/catkin_lint.yml b/.github/workflows/catkin_lint.yml index 55961f774..c71f6ce12 100644 --- a/.github/workflows/catkin_lint.yml +++ b/.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 diff --git a/aerial_robot_base/setup.py b/aerial_robot_base/setup.py index 4640bfb98..aaffeae6d 100644 --- a/aerial_robot_base/setup.py +++ b/aerial_robot_base/setup.py @@ -3,6 +3,6 @@ setup_args = generate_distutils_setup( packages=['aerial_robot_base'], - package_dir={'': 'test'}) + package_dir={'': 'src'}) setup(**setup_args) diff --git a/aerial_robot_base/test/aerial_robot_base/__init__.py b/aerial_robot_base/src/aerial_robot_base/__init__.py similarity index 100% rename from aerial_robot_base/test/aerial_robot_base/__init__.py rename to aerial_robot_base/src/aerial_robot_base/__init__.py diff --git a/aerial_robot_base/test/aerial_robot_base/hovering_check.py b/aerial_robot_base/src/aerial_robot_base/hovering_check.py similarity index 100% rename from aerial_robot_base/test/aerial_robot_base/hovering_check.py rename to aerial_robot_base/src/aerial_robot_base/hovering_check.py From 92fb58f39bdff1734ddb9b0e1abc5eeb11e8a49b Mon Sep 17 00:00:00 2001 From: Moju Zhao Date: Sun, 12 Jan 2025 12:26:39 +0900 Subject: [PATCH 2/9] [Robot Interface] add python interpreted interface to support basic navigation motion, including joint control --- aerial_robot_base/package.xml | 1 + .../src/aerial_robot_base/robot_interface.py | 516 ++++++++++++++++++ 2 files changed, 517 insertions(+) create mode 100644 aerial_robot_base/src/aerial_robot_base/robot_interface.py diff --git a/aerial_robot_base/package.xml b/aerial_robot_base/package.xml index e9d88bda9..c2b5d6c75 100644 --- a/aerial_robot_base/package.xml +++ b/aerial_robot_base/package.xml @@ -23,6 +23,7 @@ joy mocap_optitrack roscpp + ros_numpy rospy rostest ublox_gps diff --git a/aerial_robot_base/src/aerial_robot_base/robot_interface.py b/aerial_robot_base/src/aerial_robot_base/robot_interface.py new file mode 100644 index 000000000..545c18f02 --- /dev/null +++ b/aerial_robot_base/src/aerial_robot_base/robot_interface.py @@ -0,0 +1,516 @@ +#!/usr/bin/env python +# -*- coding: utf-8 -*- + +# Software License Agreement (BSD License) + +# Copyright (c) 2024, 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 math +import numpy as np +import rospy + +import ros_numpy as ros_np +import tf2_ros +from tf.transformations import * +import rosgraph + +from aerial_robot_msgs.msg import FlightNav, PoseControlPid +from geometry_msgs.msg import PoseStamped, Wrench, Vector3, WrenchStamped, Quaternion, QuaternionStamped +from nav_msgs.msg import Odometry +from sensor_msgs.msg import Joy, JointState +from spinal.msg import DesireCoord +from std_msgs.msg import Empty, Int8, UInt8, String +from std_srvs.srv import SetBool, SetBoolRequest + +class RobotInterface(object): + def __init__(self, robot_ns = "", debug_view = False, init_node = False): + + + # flight states: + # TODO: get from header file + self.ARM_OFF_STATE = 0 + self.START_STATE = 1 + self.ARM_ON_STATE = 2 + self.TAKEOFF_STATE = 3 + self.LAND_STATE = 4 + self.HOVER_STATE = 5 + self.STOP_STATE = 6 + + self.debug_view = debug_view + self.joint_state = JointState() + self.cog_odom = None + self.base_odom = None + self.flight_state = None + self.target_pos = np.array([0,0,0]) + + if init_node: + rospy.init_node("rotor_interface") + + self.default_pos_thresh = rospy.get_param('~default_pos_thresh', [0.1, 0.1, 0.1]) # m + self.default_rot_thresh = rospy.get_param('~default_rot_thresh', [0.05, 0.05, 0.05]) # rad + self.default_vel_thresh = rospy.get_param('~default_vel_thresh', [0.05, 0.05, 0.05]) # rad + + self.robot_ns = robot_ns + if not self.robot_ns: + master = rosgraph.Master('/rostopic') + try: + _, subs, _ = master.getSystemState() + except socket.error: + raise ROSTopicIOException("Unable to communicate with master!") + + teleop_topics = [topic[0] for topic in subs if 'teleop_command/start' in topic[0]] + if len(teleop_topics) == 1: + self.robot_ns = teleop_topics[0].split('/teleop')[0] + + # Teleop + self.start_pub = rospy.Publisher(self.robot_ns + '/teleop_command/start', Empty, queue_size = 1) + self.takeoff_pub = rospy.Publisher(self.robot_ns + '/teleop_command/takeoff', Empty, queue_size = 1) + self.land_pub = rospy.Publisher(self.robot_ns + '/teleop_command/land', Empty, queue_size = 1) + self.force_landing_pub = rospy.Publisher(self.robot_ns + '/teleop_command/force_landing', Empty, queue_size = 1) + self.halt_pub = rospy.Publisher(self.robot_ns + '/teleop_command/halt', Empty, queue_size = 1) + + # Odometry&Control + self.cog_odom_sub = rospy.Subscriber(self.robot_ns + '/uav/cog/odom', Odometry, self.cogOdomCallback) + self.base_odom_sub = rospy.Subscriber(self.robot_ns + '/uav/baselink/odom', Odometry, self.baseOdomCallback) + self.control_pid_sub = rospy.Subscriber(self.robot_ns + '/debug/pose/pid', PoseControlPid, self.controlPidCallback) + + # Navigatoin + self.flight_state_sub = rospy.Subscriber(self.robot_ns + '/flight_state', UInt8, self.flightStateCallback) + self.traj_nav_pub = rospy.Publisher(self.robot_ns + '/target_pose', PoseStamped, queue_size = 1) + self.direct_nav_pub = rospy.Publisher(self.robot_ns + '/uav/nav', FlightNav, queue_size = 1) + self.final_rot_pub = rospy.Publisher(self.robot_ns + '/final_target_baselink_rot', DesireCoord, queue_size = 1) + + # Joint + self.joint_state_sub = rospy.Subscriber(self.robot_ns + '/joint_states', JointState, self.jointStateCallback) + self.joint_ctrl_pub = rospy.Publisher(self.robot_ns + '/joints_ctrl', JointState, queue_size = 1) + self.set_joint_torque_client = rospy.ServiceProxy(self.robot_ns + '/joints/torque_enable', SetBool) + + self.tf_buffer = tf2_ros.Buffer() + self.tf_listener = tf2_ros.TransformListener(self.tf_buffer) + + start_time = rospy.get_time() + while not rospy.is_shutdown(): + elapsed_time = rospy.get_time() - start_time + if elapsed_time > 5.0: + rospy.logerr("cannot connect to {}".format(self.robot_ns)) + break + + if self.base_odom is not None: + rospy.loginfo("connect to {}!".format(self.robot_ns)) + break + + rospy.sleep(0.1) + + def convergenceCheck(self, timeout, func, *args, **kwargs): + + # if timeout is -1, it means no time constraint + if timeout < 0: + return True + + # check convergence + start_time = rospy.get_time() + + while not rospy.is_shutdown(): + + if self.flight_state != self.HOVER_STATE and self.flight_state != self.ARM_OFF_STATE: + rospy.logwarn("[{}]: preempt because current flight state({}) allows no more motion".format(func.__name__, self.flight_state)) + return False + + ret = func(*args, **kwargs) + + if ret: + rospy.loginfo("[{}]: convergence".format(func.__name__)) + return True + + elapsed_time = rospy.get_time() - start_time + if elapsed_time > timeout: + rospy.logwarn("[{}]: timeout, cannot convergence".format(func.__name__)) + return False + + rospy.sleep(0.1) + + return False + + def start(self, sleep = 1.0): + self.start_pub.publish() + rospy.sleep(sleep) + + def takeoff(self): + self.takeoff_pub.publish() + + def land(self): + self.land_pub.publish() + + def forceLanding(self): + self.force_landing_pub.publish() + + def halt(self): + self.halt_pub.publish() + + def baseOdomCallback(self, msg): + self.base_odom = msg + + def cogOdomCallback(self, msg): + self.cog_odom = msg + + def flightStateCallback(self, msg): + self.flight_state = msg.data + + def controlPidCallback(self, msg): + self.control_pid = msg + + def getControlPid(self): + return self.control_pid + + def getBaseOdom(self): + return self.base_odom + + def getCogOdom(self): + return self.cog_odom + + def getBasePos(self): + return ros_np.numpify(self.base_odom.pose.pose.position) + + def getBaseRot(self): + return ros_np.numpify(self.base_odom.pose.pose.orientation) + + def getBaseRPY(self): + return euler_from_quaternion(self.getBaseRot()) + + def getBaseLinearVel(self): + return ros_np.numpify(self.base_odom.twist.twist.linear) + + def getBaseAngularVel(self): + return ros_np.numpify(self.base_odom.twist.twist.angular) + + def getCogPos(self): + return ros_np.numpify(self.cog_odom.pose.pose.position) + + def getCogRot(self): + return ros_np.numpify(self.cog_odom.pose.pose.orientation) + + def getCogRPY(self): + return euler_from_quaternion(self.getCogRot()) + + def getCogLinVel(self): + return ros_np.numpify(self.cog_odom.twist.twist.linear) + + def getCogAngVel(self): + return ros_np.numpify(self.cog_odom.twist.twist.angular) + + def getFlightState(self): + return self.flight_state + + def getTargetPos(self): + return self.target_pos + + def goPos(self, pos, pos_thresh = 0.1, vel_thresh = 0.05, timeout = 30): + + return self.navigate(pos = pos, pos_thresh = pos_thresh, vel_thresh = vel_thresh, timeout = timeout) + + def rotateYaw(self, yaw, yaw_thresh = 0.1, timeout = 30): + + rot = quaternion_from_euler(0, 0, yaw) + return self.rotate(rot, yaw_thresh, timeout) + + def rotate(self, rot, rot_thresh = 0.1, timeout = 30): + + return self.navigate(rot = rot, rot_thresh = rot_thresh, timeout = timeout) + + def goPosYaw(self, pos, yaw, pos_thresh = 0.1, vel_thresh = 0.05, yaw_thresh = 0.1, timeout = 30): + + rot = quaternion_from_euler(0, 0, yaw) + return self.goPose(pos, rot, pos_thresh, vel_thresh, yaw_thresh, timeout) + + def goPose(self, pos, rot, pos_thresh = 0.1, vel_thresh = 0.05, rot_thresh = 0.1, timeout = 30): + + return self.navigate(pos = pos, rot = rot, pos_thresh = pos_thresh, vel_thresh = vel_thresh, rot_thresh = rot_thresh, timeout = timeout) + + def goVel(self, vel): + return self.navigate(lin_vel = vel) + + def rotateVel(self, vel): + return self.navigate(ang_vel = vel) + + def trajectoryNavigate(self, pos, rot): + + if self.flight_state != self.HOVER_STATE: + rospy.logerr("[Navigate] the current flight state({}) does not allow navigation".format(self.flight_state)) + return + + if pos is None: + pos = self.getCogPos() + if rot is None: + rot = self.getCogRot() + + msg = PoseStamped() + msg.header.stamp = rospy.Time.now() + msg.pose.position.x = pos[0] + msg.pose.position.y = pos[1] + msg.pose.position.z = pos[2] + msg.pose.orientation.x = rot[0] + msg.pose.orientation.y = rot[1] + msg.pose.orientation.z = rot[2] + msg.pose.orientation.w = rot[3] + self.traj_nav_pub.publish(msg) + + + def directNavigate(self, pos, rot, lin_vel, ang_vel): + + if self.flight_state != self.HOVER_STATE: + rospy.logerr("[Navigate] the current flight state({}) does not allow navigation".format(self.flight_state)) + return + + msg = FlightNav() + msg.header.stamp = rospy.Time.now() + + msg.control_frame = FlightNav.WORLD_FRAME + msg.target = FlightNav.COG + + pos_mode = FlightNav.NO_NAVIGATION + if pos is None: + if lin_vel is not None: + pos_mode = FlightNav.VEL_MODE + else: + if lin_vel is None: + pos_mode = FlightNav.POS_MODE + else: + pos_mode = FlightNav.POS_VEL_MODE + + rot_mode = FlightNav.NO_NAVIGATION + if rot is None: + if ang_vel is not None: + rot_mode = FlightNav.VEL_MODE + else: + if ang_vel is None: + rot_mode = FlightNav.POS_MODE + else: + rot_mode = FlightNav.POS_VEL_MODE + + + if pos is None: + pos = self.getCogPos() + + if rot is None: + rot = self.getCogRot() + _, _, yaw = euler_from_quaternion(rot) + + if lin_vel is None: + lin_vel = np.array([0, 0, 0]) + + if ang_vel is None: + ang_vel = np.array([0, 0, 0]) + + msg.pos_xy_nav_mode = pos_mode + msg.target_pos_x = pos[0] + msg.target_pos_y = pos[1] + msg.target_vel_x = lin_vel[0] + msg.target_vel_y = lin_vel[1] + msg.pos_z_nav_mode = pos_mode + msg.target_pos_z = pos[2] + msg.target_vel_z = lin_vel[2] + msg.yaw_nav_mode = rot_mode + msg.target_yaw = yaw + msg.target_omega_z = ang_vel[2] + + self.direct_nav_pub.publish(msg) + + def navigate(self, pos = None, rot = None, lin_vel = None, ang_vel = None, pos_thresh = 0.1, vel_thresh = 0, rot_thresh = 0, timeout = -1): + + if self.flight_state != self.HOVER_STATE: + rospy.logerr("[Navigate] the current flight state({}) does not allow navigation".format(self.flight_state)) + return False + + if rot is not None: + + # change rpy to quaternion + if len(rot) == 3: + rot = quaternion_from_euler(*rot) + + # check whether the pose is final one + if lin_vel is None and ang_vel is None: + self.trajectoryNavigate(pos, rot) + else: + self.directNavigate(pos, rot, lin_vel, ang_vel) + + return self.poseConvergenceCheck(timeout, target_pos = pos, target_rot = rot, pos_thresh = pos_thresh, vel_thresh = vel_thresh, rot_thresh = rot_thresh) + + def poseConvergenceCheck(self, timeout, target_pos = None, target_rot = None, pos_thresh = None, vel_thresh = None, rot_thresh = None): + + return self.convergenceCheck(timeout, self.poseThresholdCheck, target_pos, target_rot, pos_thresh, vel_thresh, rot_thresh) + + def poseThresholdCheck(self, target_pos = None, target_rot = None, pos_thresh = None, vel_thresh = None, rot_thresh = None): + + # threshold + if pos_thresh is None: + pos_thresh = self.default_pos_thresh + + if isinstance(pos_thresh, float): + pos_thresh = [pos_thresh] * 3 + + if rot_thresh is None: + rot_thresh = self.default_rot_thresh + + if isinstance(rot_thresh, float): + rot_thresh = [rot_thresh] * 3 + + if vel_thresh is None: + vel_thresh = self.default_vel_thresh + + if isinstance(vel_thresh, float): + vel_thresh = [vel_thresh] * 3 + + # target pose + if target_pos is None: + target_pos = self.getCogPos() + pos_thresh = np.array([1e6] * 3) + vel_thresh = np.array([1e6] * 3) + + if target_rot is None: + target_rot = self.getBaseRot() # assume the coordinate axes of baselink are identical to those of CoG + rot_thresh = np.array([1e6] * 3) + if len(target_rot) == 3: + target_rot = quaternion_from_euler(*target_rot) + + + # current pose + current_pos = self.getCogPos() + current_vel = self.getCogLinVel() + current_rot = self.getBaseRot() # assume the coordinate axes of baselink are identical to those of CoG + + # delta_pos + delta_pos = target_pos - current_pos + delta_vel = current_vel + delta_rot = quaternion_multiply(quaternion_inverse(current_rot), \ + target_rot) + delta_rot = euler_from_quaternion(delta_rot) + + rospy.loginfo_throttle(1.0, 'pose diff: {}, rot: {}, vel: {}; target pose: pos: {}, rot: {}; current pose: \n pos: {}, rot: {}, vel: {}'.format(delta_pos, delta_rot, delta_vel, target_pos, target_rot, current_pos, current_rot, current_vel)) + + if np.all(np.abs(delta_pos) < pos_thresh) and \ + np.all(np.abs(delta_rot) < rot_thresh) and \ + np.all(np.abs(delta_vel) < vel_thresh): + return True + else: + return False + + # speical rorataion function for DRAGON like robot + def rotateCog(self, roll, pitch): + # send the target roll and pitch angles + msg = DesireCoord() + msg.roll = roll + msg.pitch = pitch + self.final_rot_pub.publish(msg) + + + def getTF(self, frame_id, wait=0.5, parent_frame_id='world'): + trans = self.tf_buffer.lookup_transform(parent_frame_id, frame_id, rospy.Time.now(), rospy.Duration(wait)) + return trans + + + def jointStateCallback(self, msg): + joint_state = JointState() + + # only extract joint, exclude other component like gimbal + for n, j in zip(msg.name, msg.position): + if 'joint' not in n: + continue + joint_state.name.append(n) + joint_state.position.append(j) + self.joint_state = joint_state + + + def getJointState(self): + return self.joint_state + + def setJointAngle(self, target_joint_names, target_joint_angles, thresh = 0.05, timeout = -1): + + if self.flight_state != self.HOVER_STATE and self.flight_state != self.ARM_OFF_STATE: + rospy.logerr("[Send Joint] the current flight state({}) does not allow joint motion ".format(self.flight_state)) + return False + + + if len(target_joint_names) != len(target_joint_angles): + rospy.logerr("[Send Joint] the size of joint names {} and angles {} are not same".format(len(target_joint_names), len(target_joint_angles))) + return False + + # check whether the joint exists + for name in target_joint_names: + if name not in self.joint_state.name: + rospy.logerr("set jont angle: cannot find {}".format(name)) + return False + + # send joint angles + target_joint_state = JointState() + target_joint_state.name = target_joint_names + target_joint_state.position = target_joint_angles + self.joint_ctrl_pub.publish(target_joint_state) + + return self.jointConvergenceCheck(timeout, target_joint_names, target_joint_angles, thresh) + + def jointConvergenceCheck(self, timeout, target_joint_names, target_joint_angles, thresh): + + return self.convergenceCheck(timeout, self.jointThresholdCheck, target_joint_names, target_joint_angles, thresh) + + def jointThresholdCheck(self, target_joint_names, target_joint_angles, thresh): + + if len(target_joint_names) != len(target_joint_angles): + rospy.logerr("[Send Joint] the sizes of joint names {} and angles {} are not same".format(len(target_joint_names), len(target_joint_angles))) + return False + + # check whether the joint exists, and create index map + index_map = [] + for i, name in enumerate(target_joint_names): + try: + j = self.joint_state.name.index(name) + except ValueError: + rospy.logerr("set jont angle: cannot find {}".format(name)) + return False + + index_map.append(j) + + delta_ang = [] + for index, target_ang in zip(index_map, target_joint_angles): + current_ang = self.joint_state.position[index] + delta = target_ang - current_ang + delta_ang.append(delta) + + rospy.loginfo_throttle(1.0, "delta angle: {}".format(delta_ang)) + + if np.all(np.abs(delta_ang) < thresh): + return True + else: + return False + + def setJointTorque(self, state): + req = SetBoolRequest() + req.data = state + try: + self.set_joint_torque_client(req) + except rospy.ServiceException as e: + print("Service call failed: %s"%e) From f15e3fbb76742cb745e22966026081fe37dccff2 Mon Sep 17 00:00:00 2001 From: Moju Zhao Date: Sun, 12 Jan 2025 11:45:54 +0900 Subject: [PATCH 3/9] [Navigation] refactor the process for landing phase. Only accept new pose in hover state and also reset the target velocity in landing phase --- .../src/flight_navigation.cpp | 56 ++++++++++--------- 1 file changed, 29 insertions(+), 27 deletions(-) diff --git a/aerial_robot_control/src/flight_navigation.cpp b/aerial_robot_control/src/flight_navigation.cpp index 42c7c47b6..3ef37bd67 100644 --- a/aerial_robot_control/src/flight_navigation.cpp +++ b/aerial_robot_control/src/flight_navigation.cpp @@ -145,6 +145,8 @@ void BaseNavigator::batteryCheckCallback(const std_msgs::Float32ConstPtr &msg) void BaseNavigator::poseCallback(const geometry_msgs::PoseStampedConstPtr & msg) { + if(getNaviState() != HOVER_STATE) return; + if (traj_generator_ptr_.get() == nullptr) { ROS_DEBUG("traj_generator_ptr_ is null"); @@ -155,6 +157,8 @@ void BaseNavigator::poseCallback(const geometry_msgs::PoseStampedConstPtr & msg) void BaseNavigator::simpleMoveBaseGoalCallback(const geometry_msgs::PoseStampedConstPtr & msg) { + if(getNaviState() != HOVER_STATE) return; + if (traj_generator_ptr_.get() == nullptr) { ROS_DEBUG("traj_generator_ptr_ is null"); @@ -167,7 +171,7 @@ void BaseNavigator::simpleMoveBaseGoalCallback(const geometry_msgs::PoseStampedC void BaseNavigator::naviCallback(const aerial_robot_msgs::FlightNavConstPtr & msg) { - if(getNaviState() == TAKEOFF_STATE || getNaviState() == LAND_STATE) return; + if(getNaviState() != HOVER_STATE) return; gps_waypoint_ = false; @@ -819,36 +823,29 @@ void BaseNavigator::update() } case LAND_STATE: { - if (getNaviState() > START_STATE) - { - updateLandCommand(); + updateLandCommand(); - if (ros::Time::now().toSec() - land_check_start_time_ > land_check_duration_) - { - double delta = curr_pos.z() - land_height_; - double vel = curr_vel.z(); - - ROS_INFO("expected land height: %f (current height: %f), velocity: %f ", land_height_, curr_pos.z(), vel); + if (ros::Time::now().toSec() - land_check_start_time_ > land_check_duration_) + { + double delta = curr_pos.z() - land_height_; + double vel = curr_vel.z(); - if (fabs(delta) < land_pos_convergent_thresh_ && - vel > -land_vel_convergent_thresh_) - { - ROS_INFO("\n \n ====================== \n Land !!! \n ====================== \n"); - ROS_INFO("Start disarming motors"); - setNaviState(STOP_STATE); - } - else - { - // not staedy, update the land height - land_height_ = curr_pos.z(); - } + ROS_INFO("expected land height: %f (current height: %f), velocity: %f ", land_height_, curr_pos.z(), vel); - land_check_start_time_ = ros::Time::now().toSec(); + if (fabs(delta) < land_pos_convergent_thresh_ && + vel > -land_vel_convergent_thresh_) + { + ROS_INFO("\n \n ====================== \n Land !!! \n ====================== \n"); + ROS_INFO("Start disarming motors"); + setNaviState(STOP_STATE); } - } - else - { - setNaviState(ARM_OFF_STATE); + else + { + // not staedy, update the land height + land_height_ = curr_pos.z(); + } + + land_check_start_time_ = ros::Time::now().toSec(); } break; } @@ -960,6 +957,11 @@ void BaseNavigator::updateLandCommand() addTargetPosZ(land_descend_vel_ * loop_du_); setTargetVelZ(land_descend_vel_); + + // update vel for other axes + setTargetVelX(0); + setTargetVelY(0); + setTargetOmega(0, 0, 0); } From a65dd683652a0635507afe63871b86d7ab6c1282 Mon Sep 17 00:00:00 2001 From: Moju Zhao Date: Sun, 12 Jan 2025 11:53:50 +0900 Subject: [PATCH 4/9] [Dragon][Naviagation] refactor the landing process by introducing a new state: pre_land_state --- .../dragon/include/dragon/dragon_navigation.h | 6 ++-- .../dragon/src/control/lqi_gimbal_control.cpp | 6 ++-- robots/dragon/src/dragon_navigation.cpp | 29 +++++++++---------- 3 files changed, 19 insertions(+), 22 deletions(-) diff --git a/robots/dragon/include/dragon/dragon_navigation.h b/robots/dragon/include/dragon/dragon_navigation.h index 92ed86255..16305e6e9 100644 --- a/robots/dragon/include/dragon/dragon_navigation.h +++ b/robots/dragon/include/dragon/dragon_navigation.h @@ -54,8 +54,6 @@ namespace aerial_robot_navigation void update() override; - inline const bool getLandingFlag() const { return landing_flag_; } - private: ros::Publisher curr_target_baselink_rot_pub_; ros::Publisher joint_control_pub_; @@ -81,7 +79,6 @@ namespace aerial_robot_navigation /* landing process */ bool level_flag_; - bool landing_flag_; bool servo_torque_; /* rosparam */ @@ -89,5 +86,8 @@ namespace aerial_robot_navigation string joints_torque_control_srv_name_, gimbals_torque_control_srv_name_; double baselink_rot_change_thresh_; double baselink_rot_pub_interval_; + + // addtional state + static constexpr uint8_t PRE_LAND_STATE = 0x20; }; }; diff --git a/robots/dragon/src/control/lqi_gimbal_control.cpp b/robots/dragon/src/control/lqi_gimbal_control.cpp index e332735c1..f5215977f 100644 --- a/robots/dragon/src/control/lqi_gimbal_control.cpp +++ b/robots/dragon/src/control/lqi_gimbal_control.cpp @@ -225,8 +225,8 @@ void DragonLQIGimbalController::gimbalControl() std::cout << "gimbal force for horizontal control:" << std::endl << f_xy << std::endl; } - /* external wrench compensation */ - if(boost::dynamic_pointer_cast(navigator_)->getLandingFlag()) + /* clear external wrench compensation */ + if(navigator_->getNaviState() != aerial_robot_navigation::HOVER_STATE) { dragon_robot_model_->resetExternalStaticWrench(); // clear the external wrench extra_vectoring_force_.setZero(); // clear the extra vectoring force @@ -321,7 +321,7 @@ bool DragonLQIGimbalController::clearExternalWrenchCallback(gazebo_msgs::BodyReq /* extra vectoring force */ void DragonLQIGimbalController::extraVectoringForceCallback(const std_msgs::Float32MultiArrayConstPtr& msg) { - if(navigator_->getNaviState() != aerial_robot_navigation::HOVER_STATE || navigator_->getForceLandingFlag() || boost::dynamic_pointer_cast(navigator_)->getLandingFlag()) return; + if(navigator_->getNaviState() != aerial_robot_navigation::HOVER_STATE || navigator_->getForceLandingFlag()) return; if(extra_vectoring_force_.size() != msg->data.size()) { diff --git a/robots/dragon/src/dragon_navigation.cpp b/robots/dragon/src/dragon_navigation.cpp index d75a9538a..8cbc0ddcb 100644 --- a/robots/dragon/src/dragon_navigation.cpp +++ b/robots/dragon/src/dragon_navigation.cpp @@ -7,7 +7,6 @@ DragonNavigator::DragonNavigator(): BaseNavigator(), servo_torque_(false), level_flag_(false), - landing_flag_(false), curr_target_baselink_rot_(0, 0, 0), final_target_baselink_rot_(0, 0, 0) { @@ -86,7 +85,6 @@ void DragonNavigator::landingProcess() } } - joint_control_pub_.publish(joint_control_msg); final_target_baselink_rot_.setValue(0, 0, 0); @@ -98,23 +96,23 @@ void DragonNavigator::landingProcess() /* force set the current deisre tilt to current estimated tilt */ curr_target_baselink_rot_.setValue(curr_roll, curr_pitch, 0); - } - level_flag_ = true; + if(getNaviState() == LAND_STATE) + { + setNaviState(PRE_LAND_STATE); + setTeleopFlag(false); + setTargetPosZ(estimator_->getState(State::Z_COG, estimate_mode_)[0]); + setTargetVelZ(0); + land_height_ = 0; // reset the land height, since it is updated in the first land_state which is forced to change to hover state to level the orientation. Thus, it is possible to have the same land height just after switching back to land state and thus stop in midair + ROS_INFO("[Navigation] shift to pre_land state to make the robot level"); + } - if(getNaviState() == LAND_STATE && !landing_flag_) - { - landing_flag_ = true; - setTeleopFlag(false); - setTargetPosZ(estimator_->getState(State::Z_COG, estimate_mode_)[0]); - setTargetVelZ(0); - land_height_ = 0; // reset the land height, since it is updated in the first land_state which is forced to change to hover state to level the orientation. Thus, it is possible to have the same land height just after switching back to land state and thus stop in midair - setNaviState(HOVER_STATE); + level_flag_ = true; } } /* back to landing process */ - if(landing_flag_) + if(getNaviState() == PRE_LAND_STATE) { const auto joint_state = robot_model_->kdlJointToMsg(robot_model_->getJointPositions()); bool already_level = true; @@ -129,9 +127,9 @@ void DragonNavigator::landingProcess() if(curr_target_baselink_rot_.length()) already_level = false; - if(already_level && getNaviState() == HOVER_STATE) + if(already_level) { - ROS_WARN("gimbal control: back to land state"); + ROS_INFO("[Navigation] back to land state"); setNaviState(LAND_STATE); setTeleopFlag(true); } @@ -188,7 +186,6 @@ void DragonNavigator::reset() BaseNavigator::reset(); level_flag_ = false; - landing_flag_ = false; } From 319fbf1f8cfee524d87d8475442931124009a045 Mon Sep 17 00:00:00 2001 From: Moju Zhao Date: Sun, 12 Jan 2025 11:58:44 +0900 Subject: [PATCH 5/9] [Dragon] remove unnecessary scripts --- robots/dragon/scripts/desired_control_test.py | 27 ------ robots/dragon/scripts/dummy_joint_state.py | 43 --------- robots/dragon/scripts/gimbal_control_test.py | 37 -------- robots/dragon/scripts/joint_control_test.py | 82 ----------------- .../real_machine_joint_state_bridge.py | 91 ------------------- 5 files changed, 280 deletions(-) delete mode 100755 robots/dragon/scripts/desired_control_test.py delete mode 100755 robots/dragon/scripts/dummy_joint_state.py delete mode 100755 robots/dragon/scripts/gimbal_control_test.py delete mode 100755 robots/dragon/scripts/joint_control_test.py delete mode 100755 robots/dragon/scripts/real_machine_joint_state_bridge.py diff --git a/robots/dragon/scripts/desired_control_test.py b/robots/dragon/scripts/desired_control_test.py deleted file mode 100755 index cd016851e..000000000 --- a/robots/dragon/scripts/desired_control_test.py +++ /dev/null @@ -1,27 +0,0 @@ -#!/usr/bin/env python - -import sys -import time -import rospy -import math -from sensor_msgs.msg import JointState - -rospy.init_node("gimbal_control_test") - -from spinal.msg import DesireCoord - -att_control_pub = rospy.Publisher("/dragon/final_target_baselink_rot", DesireCoord, queue_size=1) - -desire_att = DesireCoord() - -time.sleep(0.5) - -while not rospy.is_shutdown(): - - if desire_att.roll == -1.57: - desire_att.roll = 0 - else: - desire_att.roll = -1.57 - att_control_pub.publish(desire_att) - - time.sleep(10) diff --git a/robots/dragon/scripts/dummy_joint_state.py b/robots/dragon/scripts/dummy_joint_state.py deleted file mode 100755 index 2f0370602..000000000 --- a/robots/dragon/scripts/dummy_joint_state.py +++ /dev/null @@ -1,43 +0,0 @@ -#!/usr/bin/env python - -import sys -import time -import rospy -import math -from sensor_msgs.msg import JointState - -rospy.init_node("dragon_dummy_joint_state") - -link_num = rospy.get_param("~link_num", 4) -duration = rospy.get_param("~duration", 0.05) #20Hz - -joint_control_topic_name = rospy.get_param("~joint_state_topic_name", "/dragon/joint_states") -pub = rospy.Publisher(joint_control_topic_name, JointState, queue_size=10) - -joint = JointState() -joint.name = [] -joint.position = [] - -# gimbal -for i in range(1, link_num + 1): - joint.name.append("gimbal" + str(i) + "_pitch") - joint.name.append("gimbal" + str(i) + "_roll") - joint.position.append(0) - joint.position.append(0) - -# joint -for i in range(1, link_num): - joint.name.append("joint" + str(i) + "_pitch") - joint.name.append("joint" + str(i) + "_yaw") - joint.position.append(0) - if i == 1: - joint.position.append(1.57) - if i == 2: - joint.position.append(0) - if i == 3: - joint.position.append(0) - -while not rospy.is_shutdown(): - joint.header.stamp = rospy.get_rostime() - pub.publish(joint) - time.sleep(duration) diff --git a/robots/dragon/scripts/gimbal_control_test.py b/robots/dragon/scripts/gimbal_control_test.py deleted file mode 100755 index e6e27ea43..000000000 --- a/robots/dragon/scripts/gimbal_control_test.py +++ /dev/null @@ -1,37 +0,0 @@ -#!/usr/bin/env python - -import sys -import time -import rospy -import math -from sensor_msgs.msg import JointState - -rospy.init_node("gimbal_control_test") - -link_num = rospy.get_param("~link_num", 4) -duration = rospy.get_param("~duration", 0.05) #20Hz - -gimbal_control_topic_name = rospy.get_param("~gimbal_control_topic_name", "/dragon/gimbals_ctrl") -pub = rospy.Publisher(gimbal_control_topic_name, JointState, queue_size=10) - -gimbal = JointState() -gimbal.position = [] - -# gimbal -for i in range(1, link_num + 1): - gimbal.position.append(0) - gimbal.position.append(0) - -gimbal.position[0] = 0.6; -pub.publish(gimbal) - -time.sleep(0.5) - -start_time = rospy.get_time() -while not rospy.is_shutdown(): - time_diff = start_time - rospy.get_time() - gimbal.position[0] = 0.6 * math.cos(8 * time_diff / math.pi / 2 ) - gimbal.position[1] = 0.6 * math.sin(8 * time_diff / math.pi / 2 ) - - pub.publish(gimbal) - time.sleep(duration) diff --git a/robots/dragon/scripts/joint_control_test.py b/robots/dragon/scripts/joint_control_test.py deleted file mode 100755 index d616de863..000000000 --- a/robots/dragon/scripts/joint_control_test.py +++ /dev/null @@ -1,82 +0,0 @@ -#!/usr/bin/env python - -import sys -import time -import rospy -import math -from sensor_msgs.msg import JointState - -def joint_state_cb(data): - global start_flag - global joint_state - global ctrl_joint - - joint_state = [] - for i in range(0, len(data.position)): - if "joint" in data.name[i] : - joint_state.append(data.position[i]) - - if start_flag == True: - return - - for i in range(0, len(data.position)): - if "joint" in data.name[i] : - ctrl_joint.position.append(data.position[i]) - - start_flag = True - -if __name__=="__main__": - rospy.init_node("joint_control_test") - - start_flag = False - joint_state = JointState() - joint_size = 0 - - duration = rospy.get_param("~duration", 0.05) #20Hz - - joint_control_topic_name = rospy.get_param("~joint_control_topic_name", "/dragon/joints_ctrl") - pub = rospy.Publisher(joint_control_topic_name, JointState, queue_size=10) - joint_state_topic_name = rospy.get_param("~joint_state_topic_name", "/dragon/joint_states") - rospy.Subscriber(joint_state_topic_name, JointState, joint_state_cb) - - start_time = 0 - - target_max_pitch = -0.34 - ctrl_joint = JointState() - ctrl_joint.position = [] - - init_step = True - - while not rospy.is_shutdown(): - - if start_flag == False: - continue - - if init_step == True: - - if len(joint_state) == 0: - continue - - if abs(joint_state[1]) > 0.07 or abs(joint_state[0] - target_max_pitch) > 0.07: - if abs(joint_state[1]) > 0.05: - rospy.logwarn("the joint 1 yaw is too big: %f", joint_state[1]) - - if abs(joint_state[0] - target_max_pitch) > 0.05: - rospy.logwarn("the joint 1 pitch is too small %f", joint_state[0]) - - ctrl_joint.position[0] = target_max_pitch - ctrl_joint.position[1] = 0 - - pub.publish(ctrl_joint) - time.sleep(duration) - else: - init_step = False - start_time = rospy.get_time() - continue - - time_diff = start_time - rospy.get_time() - ctrl_joint.position[0] = -0.34 * abs(math.cos(time_diff / math.pi )) - ctrl_joint.position[1] = 0.78 * math.sin(2 * time_diff / math.pi / 2 ) - - pub.publish(ctrl_joint) - time.sleep(duration) diff --git a/robots/dragon/scripts/real_machine_joint_state_bridge.py b/robots/dragon/scripts/real_machine_joint_state_bridge.py deleted file mode 100755 index d8c89eefd..000000000 --- a/robots/dragon/scripts/real_machine_joint_state_bridge.py +++ /dev/null @@ -1,91 +0,0 @@ -#!/usr/bin/env python - -import sys -import time -import rospy -import tf -import math -from geometry_msgs.msg import TransformStamped -from sensor_msgs.msg import JointState -from tf import transformations as t - -def gimbal_state_cb(data): - global gimbal_start_flag - global gimbal_state - - if not gimbal_start_flag: - for i in range(0, len(data.position)): - if i % 2 == 0: - gimbal_state.name.append("gimbal" + str(i / 2 + 1) + "_roll") # e.g. gimbal1_roll - else: - gimbal_state.name.append("gimbal" + str(i / 2 + 1) + "_pitch") # e.g. gimbal1_pitch - - gimbal_state.position.append(data.position[i]) - - gimbal_start_flag = True - else: - for i in range(0, len(data.position)): - gimbal_state.position[i] = data.position[i] - -def cog2baselink_cb(data): - - t = tf.Transformer(True, rospy.Duration(10.0)) - t.setTransform(data) - (inv_trans, inv_rot) = t.lookupTransform('fc', 'cog', rospy.Time(0)) - - br = tf.TransformBroadcaster() - br.sendTransform(inv_trans, - inv_rot, - data.header.stamp, - data.header.frame_id, - data.child_frame_id) - -def joint_state_cb(data): - global joint_start_flag - global joint_state - - if not joint_start_flag: - for i in range(0, len(data.name)): - joint_state.name.append(data.name[i]) - joint_state.position.append(data.position[i]) - - #rospy.logwarn("joint name: %s", data.name[i]) - joint_start_flag = True - else: - for i in range(0, len(data.name)): - joint_state.name[i] = data.name[i] - joint_state.position[i] = data.position[i] - - joint_state.header = data.header - -if __name__=="__main__": - rospy.init_node("joint_control_test") - - gimbal_start_flag = False - joint_start_flag = False - gimbal_state = JointState() - joint_state = JointState() - - duration = rospy.get_param("~duration", 0.05) #20Hz - - gimbal_state_topic_name = rospy.get_param("~gimbal_control_topic_name", "/dragon/gimbals_ctrl") - rospy.Subscriber(gimbal_state_topic_name, JointState, gimbal_state_cb) - joint_state_temp_topic_name = rospy.get_param("~joint_state_topic_name", "/dragon/joint_states_temp") - rospy.Subscriber(joint_state_temp_topic_name, JointState, joint_state_cb) - - joint_state_topic_name = rospy.get_param("~joint_state_topic_name", "/dragon/joint_states") - pub = rospy.Publisher(joint_state_topic_name, JointState, queue_size=10) - - cog2baselink_topic_name = rospy.get_param("~cog2baselink_topic_name", "/cog2baselink") - rospy.Subscriber(cog2baselink_topic_name, TransformStamped, cog2baselink_cb) - - while not rospy.is_shutdown(): - - if not gimbal_start_flag or not joint_start_flag: - continue - - for i in range(0, len(gimbal_state.name)): - joint_state.position[joint_state.name.index(gimbal_state.name[i])] = gimbal_state.position[i] - - pub.publish(joint_state) - time.sleep(duration) From 6729a1da526e0afbfacb678ac84de14f14f6ad26 Mon Sep 17 00:00:00 2001 From: Moju Zhao Date: Sun, 12 Jan 2025 12:29:53 +0900 Subject: [PATCH 6/9] [State Machine] implement basic classes based on ros smach, and also crate a simple demo script. --- aerial_robot_base/package.xml | 2 + aerial_robot_base/scripts/simple_demo.py | 95 ++++ .../src/aerial_robot_base/state_machine.py | 472 ++++++++++++++++++ 3 files changed, 569 insertions(+) create mode 100755 aerial_robot_base/scripts/simple_demo.py create mode 100644 aerial_robot_base/src/aerial_robot_base/state_machine.py diff --git a/aerial_robot_base/package.xml b/aerial_robot_base/package.xml index c2b5d6c75..a68b10fab 100644 --- a/aerial_robot_base/package.xml +++ b/aerial_robot_base/package.xml @@ -22,6 +22,8 @@ aerial_robot_model 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' From 22eca8dd712fdd6b223a0ea2454ac8f0ab534e46 Mon Sep 17 00:00:00 2001 From: Moju Zhao Date: Sun, 12 Jan 2025 12:34:28 +0900 Subject: [PATCH 7/9] [Demo] creates more simple demo based on smach and integrate the script in each bringup launch --- robots/dragon/launch/bringup.launch | 4 + robots/dragon/scripts/simple_demo.py | 128 ++++++++++++++++++++ robots/hydrus/launch/bringup.launch | 4 + robots/hydrus/scripts/loop_demo.py | 115 ++++++++++++++++++ robots/hydrus/scripts/simple_demo.py | 109 +++++++++++++++++ robots/hydrus_xi/launch/bringup.launch | 4 + robots/mini_quadrotor/launch/bringup.launch | 5 + 7 files changed, 369 insertions(+) create mode 100755 robots/dragon/scripts/simple_demo.py create mode 100755 robots/hydrus/scripts/loop_demo.py create mode 100755 robots/hydrus/scripts/simple_demo.py 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/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/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_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/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 ######### + + + From d2846e74fb1ca969e2f7da5e5c6ec7c90853c92a Mon Sep 17 00:00:00 2001 From: Moju Zhao Date: Sun, 12 Jan 2025 12:37:02 +0900 Subject: [PATCH 8/9] [RosTest] refactor the flight control test script to be based on the new state machine system --- .github/workflows/catkin_lint.yml | 2 +- .../src/aerial_robot_base/hovering_check.py | 122 ------------- aerial_robot_base/test/hovering_check.py | 112 ++++++++++++ robots/dragon/test/dragon_control.test | 109 +++++------- robots/dragon/test/flight_check.py | 164 ++++++++++++++++++ robots/hydrus/test/flight_check.py | 164 ++++++++++++++++++ robots/hydrus/test/hydrus_control.test | 49 +++--- robots/hydrus/test/tilted_hydrus_control.test | 45 ++--- .../test/hydrus_xi_hex_branch_control.test | 26 +-- .../mini_quadrotor/test/flight_control.test | 18 +- 10 files changed, 540 insertions(+), 271 deletions(-) delete mode 100755 aerial_robot_base/src/aerial_robot_base/hovering_check.py create mode 100755 aerial_robot_base/test/hovering_check.py create mode 100755 robots/dragon/test/flight_check.py create mode 100755 robots/hydrus/test/flight_check.py diff --git a/.github/workflows/catkin_lint.yml b/.github/workflows/catkin_lint.yml index c71f6ce12..55961f774 100644 --- a/.github/workflows/catkin_lint.yml +++ b/.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 --skip-path aerial_robot_base + 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 diff --git a/aerial_robot_base/src/aerial_robot_base/hovering_check.py b/aerial_robot_base/src/aerial_robot_base/hovering_check.py deleted file mode 100755 index f6a5489b8..000000000 --- a/aerial_robot_base/src/aerial_robot_base/hovering_check.py +++ /dev/null @@ -1,122 +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. - -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") diff --git a/aerial_robot_base/test/hovering_check.py b/aerial_robot_base/test/hovering_check.py new file mode 100755 index 000000000..4ef524974 --- /dev/null +++ b/aerial_robot_base/test/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/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/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/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/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]] + + From 58c5269689d51de9d91875b21fc1ffae170de322 Mon Sep 17 00:00:00 2001 From: Moju Zhao Date: Sun, 12 Jan 2025 12:37:33 +0900 Subject: [PATCH 9/9] [Demo] remove unnecessary old demo scripts --- robots/dragon/scripts/transformation_demo.py | 51 ---- .../scripts/transformation_demo_vertical.py | 42 --- robots/hydrus/scripts/hydrus_demo.py | 29 --- robots/hydrus/test/control_check.py | 245 ------------------ 4 files changed, 367 deletions(-) delete mode 100755 robots/dragon/scripts/transformation_demo.py delete mode 100755 robots/dragon/scripts/transformation_demo_vertical.py delete mode 100755 robots/hydrus/scripts/hydrus_demo.py delete mode 100755 robots/hydrus/test/control_check.py 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/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/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") -