def noNavigation(self):
nav_msg = FlightNav()
nav_msg.header.stamp = rospy.Time.now()
nav_msg.pos_xy_nav_mode = FlightNav.NO_NAVIGATION
nav_msg.psi_nav_mode = FlightNav.NO_NAVIGATION
nav_msg.pos_z_nav_mode = FlightNav.NO_NAVIGATION
self.navigation(nav_msg)
def move_to_xyz_yaw(self,
pos_x,
pos_y,
pos_z,
yaw,
override_nav_mode=None):
"""move to target x, y position"""
# send desired position
nav_msg = FlightNav()
if override_nav_mode is None:
nav_mode = self.nav_mode
else:
nav_mode = override_nav_mode
nav_msg.target = nav_msg.COG
nav_msg.pos_xy_nav_mode = nav_mode
nav_msg.target_pos_x = pos_x
nav_msg.target_pos_y = pos_y
nav_msg.pos_z_nav_mode = nav_msg.POS_MODE
nav_msg.target_pos_z = pos_z
nav_msg.psi_nav_mode = nav_msg.POS_MODE
nav_msg.target_psi = yaw
time.sleep(self.WAIT_TIME)
self.nav_control_pub.publish(nav_msg)
time.sleep(self.WAIT_TIME)
def change_yaw(self, psi):
""" change yaw """
nav_msg = FlightNav()
nav_msg.psi_nav_mode = nav_msg.POS_MODE
nav_msg.target_psi = psi
time.sleep(self.WAIT_TIME)
self.nav_control_pub.publish(nav_msg)
time.sleep(self.WAIT_TIME)
def change_height(self, pos_z):
""" change height """
nav_msg = FlightNav()
nav_msg.target = nav_msg.COG
nav_msg.pos_z_nav_mode = nav_msg.POS_MODE
nav_msg.target_pos_z = pos_z
time.sleep(self.WAIT_TIME)
self.nav_control_pub.publish(nav_msg)
time.sleep(self.WAIT_TIME)
def __init__(self):
self.period = rospy.get_param("~period", 40.0)
self.radius = rospy.get_param("~radius", 1.0)
self.init_theta = rospy.get_param("~init_theta", 0.0)
self.yaw = rospy.get_param("~yaw", True)
self.loop = rospy.get_param("~loop", False)
self.nav_pub = rospy.Publisher("uav/nav", FlightNav, queue_size=1)
self.control_sub = rospy.Subscriber("debug/pose/pid", PoseControlPid,
self.controlCb)
self.center_pos_x = None
self.center_pos_y = None
self.omega = 2 * math.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
self.flight_nav = FlightNav()
self.flight_nav.target = FlightNav.COG
self.flight_nav.pos_xy_nav_mode = FlightNav.POS_VEL_MODE
if self.yaw:
self.flight_nav.yaw_nav_mode = FlightNav.POS_VEL_MODE
signal.signal(signal.SIGINT, self.stopRequest)
time.sleep(0.5)
def __init__(self):
self.x = 0
self.y = 0
self.z = 0.5
self.step = 0.05
self.uav_nav_pub = rospy.Publisher('/uav/nav', FlightNav, queue_size=1)
self.desire_nav = FlightNav()
def goPos(self, target_xy_pos, target_z_pos):
msg = FlightNav()
msg.pos_xy_nav_mode = FlightNav.POS_MODE
msg.psi_nav_mode = FlightNav.NO_NAVIGATION
msg.pos_z_nav_mode = FlightNav.POS_MODE
msg.header.stamp = rospy.Time.now()
msg.control_frame = FlightNav.WORLD_FRAME
msg.target = FlightNav.COG
msg.target_pos_x = target_xy_pos[0]
msg.target_pos_y = target_xy_pos[1]
msg.target_pos_z = target_z_pos
self.target_xy_pos_ = target_xy_pos
self.target_z_pos_ = target_z_pos
self.uav_nav_pub_.publish(msg)
def __init__(self):
self.t = 0
self.dt = 0.001
self.z = rospy.get_param("~z", 0.5)
self.z_diff = rospy.get_param("~z_diff", 0.2)
self.curve_factor = rospy.get_param("~curve_factor", 0.01)
self.nav_control_pub = rospy.Publisher("/uav/nav",
FlightNav,
queue_size=1)
self.desire_nav = FlightNav()
def __init__(self):
self.t = 0
self.dt = 0.01
self.max = rospy.get_param("~max", 0.8)
self.min = rospy.get_param("~min", 0.5)
self.mid = (self.max + self.min) / 2.0
self.duration = rospy.get_param("~duration", 60)
self.nav_control_pub = rospy.Publisher("/uav/nav",
FlightNav,
queue_size=1000)
self.desire_nav = FlightNav()
def __init__(self):
self.t = 0
self.dt = 0.001
self.duration = rospy.get_param("~duration", 10)
self.z = rospy.get_param("~z", 0.5)
self.z_diff = rospy.get_param("~z_diff", 0.05)
self.num = 6
self.nav_control_pub = rospy.Publisher("/uav/nav",
FlightNav,
queue_size=1)
self.desire_nav = FlightNav()
while True:
tar_z = self.mid - (self.max - self.mid) * math.cos(
2 * math.pi * self.t / self.duration)
self.desire_nav.pos_z_nav_mode = 2
self.desire_nav.target_pos_z = tar_z
self.nav_control_pub.publish(self.desire_nav)
rospy.sleep(self.dt)
self.t = self.t + self.dt
if (self.t > self.duration * 3):
break
if __name__ == "__main__":
rospy.init_node("simple_flight_control_sin")
nav_control_pub = rospy.Publisher("/uav/nav", FlightNav, queue_size=1)
desire_nav = FlightNav()
desire_nav.pos_z_nav_mode = 2
desire_nav.target_pos_z = 0.5
nav_control_pub.publish(desire_nav)
rospy.sleep(5.0)
ceil_approacher = ceil_approach_nav()
ceil_approacher.tar_track_sin()
def goPos(self, frame, target_xy, target_z, target_yaw, gps_mode=False):
nav_msg = FlightNav()
if frame == 'global':
nav_msg.control_frame = nav_msg.WORLD_FRAME
elif frame == 'local':
nav_msg.control_frame = nav_msg.LOCAL_FRAME
else:
rospy.logerr('invalid frame %s' % (frame))
return
target_yaw = (target_yaw + np.pi) % (2 * np.pi) - np.pi
nav_msg.header.stamp = rospy.Time.now()
nav_msg.target = FlightNav.BASELINK
if gps_mode:
nav_msg.pos_xy_nav_mode = FlightNav.GPS_WAYPOINT_MODE
else:
nav_msg.pos_xy_nav_mode = FlightNav.POS_MODE
nav_msg.pos_z_nav_mode = FlightNav.POS_MODE
nav_msg.psi_nav_mode = FlightNav.POS_MODE
if gps_mode:
nav_msg.target_pos_x = target_xy[0]
nav_msg.target_pos_y = target_xy[1]
else:
nav_msg.target_pos_x = target_xy[0] + self.xy_pos_offset_[0]
nav_msg.target_pos_y = target_xy[1] + self.xy_pos_offset_[1]
nav_msg.target_psi = target_yaw
nav_msg.target_pos_z = target_z
self.nav_pub_.publish(nav_msg)
self.target_xy_ = target_xy
self.target_z_ = target_z
self.target_yaw_ = target_yaw
def init(self):
rospy.init_node('hydrus_tracking_controller', anonymous=True)
self.__hydrus_controller_freq = rospy.get_param('~hydrus_controller_freq', 100.0)
self.__control_mode = rospy.get_param('~control_model', POS_VEL)
self.__mpc_flag = rospy.get_param('~mpc', False)
self.__mpc_horizon = rospy.get_param('~mpc_horizon', 1.0)
if self.__mpc_flag:
rospy.loginfo("[hydrus_tracking_controller] MPC mode")
self.__control_mode = MPC
self.__hydrus_odom = Odometry()
self.__object_odom = Odometry()
self.__primitive_params = PrimitiveParams()
self.__primitive_recv_flag = False
self.__hydrus_init_process = True
self.__object_odom_sub = rospy.Subscriber("/vision/object_odom", Odometry, self.__objectOdomCallback)
self.__hydrus_odom_sub = rospy.Subscriber("/uav/cog/odom", Odometry, self.__hydrusOdomCallback)
self.__primitive_params_sub = rospy.Subscriber("/track/primitive_params", PrimitiveParams, self.__primitiveParamsCallback)
self.__hydrus_motion_init_flag_pub = rospy.Publisher('/track/hydrus_motion_init_flag', Empty, queue_size=1)
self.__hydrus_start_pub = rospy.Publisher('/teleop_command/start', Empty, queue_size=1)
self.__hydrus_takeoff_pub = rospy.Publisher('/teleop_command/takeoff', Empty, queue_size=1)
self.__hydrus_nav_cmd_pub = rospy.Publisher("/uav/nav", FlightNav, queue_size=1)
self.__hydrus_joints_ctrl_pub = rospy.Publisher("/hydrus/joints_ctrl", JointState, queue_size=1)
self.__mpc_stop_flag_pub = rospy.Publisher('/mpc/stop_cmd', Bool, queue_size=1)
self.__mpc_target_waypoints_pub = rospy.Publisher('/mpc/target_waypoints', MpcWaypointList, queue_size=1)
rospy.sleep(1.0)
self.__hydrus_nav_cmd = FlightNav()
self.__hydrus_nav_cmd.header.stamp = rospy.Time.now()
self.__hydrus_nav_cmd.header.frame_id = "world"
self.__hydrus_nav_cmd.control_frame = self.__hydrus_nav_cmd.WORLD_FRAME
self.__hydrus_nav_cmd.target = self.__hydrus_nav_cmd.COG
self.__hydrus_nav_cmd.pos_xy_nav_mode = self.__hydrus_nav_cmd.POS_MODE
self.__hydrus_nav_cmd.target_pos_x = 0.0
self.__hydrus_nav_cmd.target_pos_y = 0.0
self.__hydrus_nav_cmd.pos_z_nav_mode = self.__hydrus_nav_cmd.POS_MODE
self.__hydrus_nav_cmd.target_pos_z = 2.0
self.__hydrus_nav_cmd.psi_nav_mode = self.__hydrus_nav_cmd.POS_MODE
self.__hydrus_nav_cmd.target_psi = -math.pi / 2.0 # 0.0
self.__hydrus_joints_ctrl_msg = JointState()
self.__hydrus_joints_ctrl_msg.name.append("joint1")
self.__hydrus_joints_ctrl_msg.name.append("joint2")
self.__hydrus_joints_ctrl_msg.name.append("joint3")
self.__hydrus_joints_ctrl_msg.position.append(math.pi / 2.0)
self.__hydrus_joints_ctrl_msg.position.append(math.pi / 2.0)
self.__hydrus_joints_ctrl_msg.position.append(math.pi / 2.0)
## hydrus start and takeoff
if self.__hydrus_init_process:
rospy.sleep(1.0)
self.__hydrus_start_pub.publish(Empty())
rospy.sleep(1.0)
self.__hydrus_takeoff_pub.publish(Empty())
rospy.loginfo("Hydrus arming and takeoff command is sent.")
rospy.sleep(21.0)
rospy.loginfo("Hydrus takeoff finsihed.")
self.__hydrus_nav_cmd_pub.publish(self.__hydrus_nav_cmd)
rospy.sleep(9.0)
rospy.loginfo("Hydrus moved to initial position.")
if self.__control_mode == MPC:
mpc_stop_msg = Bool()
mpc_stop_msg.data = True
self.__mpc_stop_flag_pub.publish(mpc_stop_msg)
rospy.sleep(0.2)
self.__sendMpcTargetOdom([0.0, 0.0, 0.0], self.__mpc_horizon)
rospy.sleep(0.2)
mpc_stop_msg.data = False
self.__mpc_stop_flag_pub.publish(mpc_stop_msg)
rospy.loginfo("Start MPC control.")
rospy.sleep(0.2)
self.__mpc_start_odom = self.__hydrus_odom
self.__hydrus_motion_init_flag_pub.publish(Empty())
rospy.Timer(rospy.Duration(1.0 / self.__hydrus_controller_freq), self.__hydrusControllerCallback)
## hawk fly to the fixed position
hawk_pose_cmd_msg = PoseStamped()
hawk_pose_cmd_msg.header.frame_id = "world"
hawk_pose_cmd_msg.pose.position.x = 2.0
hawk_pose_cmd_msg.pose.position.y = 2.0
hawk_pose_cmd_msg.pose.position.z = 2.5
hawk_pose_cmd_pub.publish(hawk_pose_cmd_msg)
rospy.loginfo("guard robot flys to fixed position")
rospy.sleep(5)
rospy.loginfo("guard robot arrives")
rospy.sleep(15)
rospy.loginfo("pirate robot takeoffs")
## hydrus fly to the fixed position
hydrus_cmd_msg = FlightNav()
hydrus_cmd_msg.control_frame = hydrus_cmd_msg.WORLD_FRAME
hydrus_cmd_msg.target = hydrus_cmd_msg.COG
hydrus_cmd_msg.pos_xy_nav_mode = hydrus_cmd_msg.POS_MODE
hydrus_cmd_msg.pos_z_nav_mode = hydrus_cmd_msg.POS_MODE
hydrus_cmd_msg.psi_nav_mode = hydrus_cmd_msg.POS_MODE
hydrus_cmd_msg.target_pos_x = 2.0
hydrus_cmd_msg.target_pos_y = 2.0 - 0.6 * 0.707
hydrus_cmd_msg.target_pos_z = 2.05
hydrus_cmd_msg.target_psi = -0.7850
hydrus_nav_cmd_pub.publish(hydrus_cmd_msg)
rospy.loginfo("pirate robot flys to grub treasure")
rospy.sleep(10)
## hydrus fly back to the safe position
hydrus_cmd_msg.target_pos_x = 0.0