Skip to content

Commit

Permalink
Merge pull request #555 from tongtybj/develop/navigation/pos_vel_mode
Browse files Browse the repository at this point in the history 
[Navigation][merge after #554] improve teleoperation process
  • Loading branch information
@tongtybj
tongtybj committed Nov 18, 2023
2 parents 6a6ec73 + 3e5a2b0 commit 86b6c82
Show file tree
Hide file tree
Showing 3 changed files with 334 additions and 216 deletions.
118 changes: 95 additions & 23 deletions aerial_robot_base/scripts/keyboard_command.py
View file
Original file line number Diff line number Diff line change
@@ -1,13 +1,38 @@
#!/usr/bin/env python
import rospy

from __future__ import print_function # for print function in python2
import sys, select, termios, tty

import rospy
from std_msgs.msg import Empty
from std_msgs.msg import Int8
from std_msgs.msg import UInt16
from std_msgs.msg import UInt8
from aerial_robot_msgs.msg import FlightNav
import rosgraph

import sys, select, termios, tty



msg = """
Instruction:
---------------------------
r: arming motor (please do before takeoff)
t: takeoff
l: land
f: force landing
h: halt (force stop motor)
q w e [
(turn left) (forward) (turn right) (move up)
a s d ]
(move left) (backward) (move right) (move down)
Please don't have caps lock on.
CTRL+c to quit
---------------------------
"""

def getKey():
tty.setraw(sys.stdin.fileno())
Expand All @@ -16,10 +41,14 @@ def getKey():
termios.tcsetattr(sys.stdin, termios.TCSADRAIN, settings)
return key

def printMsg(msg, msg_len = 50):
print(msg.ljust(msg_len) + "\r", end="")

if __name__=="__main__":
settings = termios.tcgetattr(sys.stdin)
rospy.init_node("keyboard_command")
robot_ns = rospy.get_param("~robot_ns", "");
print(msg)

if not robot_ns:
master = rosgraph.Master('/rostopic')
Expand All @@ -39,43 +68,86 @@ def getKey():
start_pub = rospy.Publisher(ns + '/start', Empty, queue_size=1)
takeoff_pub = rospy.Publisher(ns + '/takeoff', Empty, queue_size=1)
force_landing_pub = rospy.Publisher(ns + '/force_landing', Empty, queue_size=1)
ctrl_mode_pub = rospy.Publisher(ns + '/ctrl_mode', Int8, queue_size=1)
motion_start_pub = rospy.Publisher('task_start', Empty, queue_size=1)
nav_pub = rospy.Publisher(robot_ns + '/uav/nav', FlightNav, queue_size=1)

xy_vel = rospy.get_param("xy_vel", 0.2)
z_vel = rospy.get_param("z_vel", 0.2)
yaw_vel = rospy.get_param("yaw_vel", 0.2)

motion_start_pub = rospy.Publisher('task_start', Empty, queue_size=1)

#the way to write publisher in python
comm=Int8()
gain=UInt16()
try:
while(True):
nav_msg = FlightNav()
nav_msg.control_frame = FlightNav.WORLD_FRAME
nav_msg.target = FlightNav.COG

key = getKey()
print("the key value is {}".format(ord(key)))
# takeoff and landing

msg = ""

if key == 'l':
land_pub.publish(Empty())
#for hydra joints
msg = "send land command"
if key == 'r':
start_pub.publish(Empty())
#for hydra joints
msg = "send motor-arming command"
if key == 'h':
halt_pub.publish(Empty())
#for hydra joints
msg = "send motor-disarming (halt) command"
if key == 'f':
force_landing_pub.publish(Empty())
msg = "send force landing command"
if key == 't':
takeoff_pub.publish(Empty())
if key == 'u':
stair_pub.publish(Empty())
msg = "send takeoff command"
if key == 'x':
motion_start_pub.publish()
if key == 'v':
comm.data = 1
ctrl_mode_pub.publish(comm)
if key == 'p':
comm.data = 0
ctrl_mode_pub.publish(comm)
msg = "send task-start command"
if key == 'w':
nav_msg.pos_xy_nav_mode = FlightNav.VEL_MODE
nav_msg.target_vel_x = xy_vel
nav_pub.publish(nav_msg)
msg = "send +x vel command"
if key == 's':
nav_msg.pos_xy_nav_mode = FlightNav.VEL_MODE
nav_msg.target_vel_x = -xy_vel
nav_pub.publish(nav_msg)
msg = "send -x vel command"
if key == 'a':
nav_msg.pos_xy_nav_mode = FlightNav.VEL_MODE
nav_msg.target_vel_y = xy_vel
nav_pub.publish(nav_msg)
msg = "send +y vel command"
if key == 'd':
nav_msg.pos_xy_nav_mode = FlightNav.VEL_MODE
nav_msg.target_vel_y = -xy_vel
nav_pub.publish(nav_msg)
msg = "send -y vel command"
if key == 'q':
nav_msg.yaw_nav_mode = FlightNav.VEL_MODE
nav_msg.target_omega_z = yaw_vel
nav_pub.publish(nav_msg)
msg = "send +yaw vel command"
if key == 'e':
nav_msg.yaw_nav_mode = FlightNav.VEL_MODE
nav_msg.target_omega_z = -yaw_vel
msg = "send -yaw vel command"
nav_pub.publish(nav_msg)
if key == '[':
nav_msg.pos_z_nav_mode = FlightNav.VEL_MODE
nav_msg.target_vel_z = z_vel
nav_pub.publish(nav_msg)
msg = "send +z vel command"
if key == ']':
nav_msg.pos_z_nav_mode = FlightNav.VEL_MODE
nav_msg.target_vel_z = -z_vel
nav_pub.publish(nav_msg)
msg = "send -z vel command"
if key == '\x03':
break

printMsg(msg)
rospy.sleep(0.001)

except Exception as e:
Expand Down
73 changes: 45 additions & 28 deletions aerial_robot_control/include/aerial_robot_control/flight_navigation.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -76,12 +76,24 @@ namespace aerial_robot_navigation
inline tf::Vector3 getTargetRPY() {return target_rpy_;}
inline tf::Vector3 getTargetOmega() {return target_omega_;}

inline void setTargetPos(tf::Vector3 pos) { target_pos_ = pos; }
inline void setTargetPos(double x, double y, double z) { setTargetPos(tf::Vector3(x, y, z)); }
inline void addTargetPos(tf::Vector3 diff_pos) { target_pos_ += diff_pos; }
inline void addTargetPos(double x, double y, double z) { addTargetPos(tf::Vector3(x, y, z)); }
inline void setTargetVel(tf::Vector3 vel) { target_vel_ = vel; }
inline void setTargetVel(double x, double y, double z) { setTargetVel(tf::Vector3(x, y, z)); }
inline void setTargetZeroVel() { setTargetVel(0,0,0); }
inline void setTargetAcc(tf::Vector3 vel) { target_acc_ = vel; }
inline void setTargetAcc(double x, double y, double z) { setTargetAcc(tf::Vector3(x, y, z)); }
inline void setTargetZeroAcc() { setTargetAcc(tf::Vector3(0,0,0)); }

inline void setTargetRoll(float value) { target_rpy_.setX(value); }
inline void setTargetOmageX(float value) { target_omega_.setX(value); }
inline void setTargetOmegaX(float value) { target_omega_.setX(value); }
inline void setTargetPitch(float value) { target_rpy_.setY(value); }
inline void setTargetOmageY(float value) { target_omega_.setY(value); }
inline void setTargetOmegaY(float value) { target_omega_.setY(value); }
inline void setTargetYaw(float value) { target_rpy_.setZ(value); }
inline void setTargetOmageZ(float value) { target_omega_.setZ(value); }
inline void addTargetYaw(float value) { setTargetYaw(angles::normalize_angle(target_rpy_.z() + value)); }
inline void setTargetOmegaZ(float value) { target_omega_.setZ(value); }
inline void setTargetPosX( float value){ target_pos_.setX(value);}
inline void setTargetVelX( float value){ target_vel_.setX(value);}
inline void setTargetAccX( float value){ target_acc_.setX(value);}
Expand Down Expand Up @@ -235,13 +247,18 @@ namespace aerial_robot_navigation
int control_frame_;
int estimate_mode_;
bool force_att_control_flag_;
bool trajectory_mode_;
bool lock_teleop_;
ros::Time force_landing_start_time_;

double hover_convergent_start_time_;
double hover_convergent_duration_;
double land_check_start_time_;
double land_check_duration_;
double trajectory_reset_time_;
double trajectory_reset_duration_;
double teleop_reset_time_;
double teleop_reset_duration_;
double z_convergent_thresh_;
double xy_convergent_thresh_;
double land_pos_convergent_thresh_;
Expand Down Expand Up @@ -271,25 +288,18 @@ namespace aerial_robot_navigation

/* teleop */
bool teleop_flag_;
bool vel_control_flag_;
bool pos_control_flag_;
bool xy_control_flag_;
bool z_control_flag_;
bool yaw_control_flag_;
bool xy_control_flag_;
bool force_landing_flag_;
bool joy_udp_;
bool check_joy_stick_heart_beat_;
bool joy_stick_heart_beat_;

double joy_target_vel_interval_;
double joy_target_z_interval_;
double max_target_vel_;
double max_target_tilt_angle_;
double max_target_yaw_rate_;

double joy_z_deadzone_;
double joy_yaw_deadzone_;
double max_teleop_xy_vel_;
double max_teleop_z_vel_;
double max_teleop_yaw_vel_;
double max_teleop_rp_angle_;

double joy_stick_deadzone_;
double joy_stick_prev_time_;
double joy_stick_heart_beat_du_;
double force_landing_to_halt_du_;
Expand All @@ -316,6 +326,7 @@ namespace aerial_robot_navigation
estimator_->setSensorFusionFlag(false);
estimator_->setFlyingFlag(false);

trajectory_mode_ = false;
init_height_ = 0;
land_height_ = 0;
}
Expand Down Expand Up @@ -358,6 +369,7 @@ namespace aerial_robot_navigation
}

setNaviState(START_STATE);
trajectory_mode_ = false;
setTargetXyFromCurrentState();
setTargetPosZ(takeoff_height_);
setTargetVelZ(0);
Expand Down Expand Up @@ -454,15 +466,15 @@ namespace aerial_robot_navigation
if(msg->data == 0)
{
setTargetXyFromCurrentState();
target_vel_.setValue(0, 0, 0);
target_acc_.setValue(0, 0, 0);
setTargetZeroVel();
setTargetZeroAcc();
xy_control_mode_ = POS_CONTROL_MODE;
ROS_INFO("x/y position control mode");
}
if(msg->data == 1)
{
target_vel_.setValue(0, 0, 0);
target_acc_.setValue(0, 0, 0);
setTargetZeroVel();
setTargetZeroAcc();
xy_control_mode_ = VEL_CONTROL_MODE;
ROS_INFO("x/y velocity control mode");
}
Expand Down Expand Up @@ -492,12 +504,12 @@ namespace aerial_robot_navigation
{
setXyControlMode(POS_CONTROL_MODE);
tf::Vector3 pos_cog = estimator_->getPos(Frame::COG, estimate_mode_);
target_pos_.setX(pos_cog.x());
target_pos_.setY(pos_cog.y());
setTargetPosX(pos_cog.x());
setTargetPosY(pos_cog.y());

// set the velocty to zero
target_vel_.setX(0);
target_vel_.setY(0);
setTargetVelX(0);
setTargetVelY(0);

// set the acceleration to zero
setTargetAccX(0);
Expand All @@ -506,18 +518,23 @@ namespace aerial_robot_navigation

void setTargetZFromCurrentState()
{
target_pos_.setZ(estimator_->getPos(Frame::COG, estimate_mode_).z());
tf::Vector3 pos_cog = estimator_->getPos(Frame::COG, estimate_mode_);
setTargetPosZ(pos_cog.z());

// set the velocty to zero
target_vel_.setZ(0);
setTargetVelZ(0);

// set the acceleration to zero
setTargetAccZ(0);
}

void setTargetYawFromCurrentState()
{
target_rpy_.setZ(estimator_->getState(State::YAW_COG, estimate_mode_)[0]);
double yaw = estimator_->getState(State::YAW_COG, estimate_mode_)[0];
setTargetYaw(yaw);

// set the velocty to zero
target_omega_.setZ(0);
setTargetOmegaZ(0);
}

template<class T> void getParam(ros::NodeHandle nh, std::string param_name, T& param, T default_value)
Expand Down
Loading

0 comments on commit 86b6c82

Please sign in to comment.