def main():
print(
"\n\nREMEMBER, NEED TO FEED THE APRILTAGS TOPIC TO THE SIMULATOR, AND HAVE THE SIMULATOR PUBLISH TO dji_sdk/tag_detections !!!! \n\n"
)
global GLOBAL_PUBLISHER
GLOBAL_PUBLISHER = rospy.Publisher('/dji_sdk/tag_detections',
AprilTagDetectionArray,
queue_size=1)
#rospy.init_node('tagPublisher', anonymous=True)
drone = DJIDrone() #this calls a rospy.init_node() function
rate = rospy.Rate(10)
global GLOBAL_DRONE
GLOBAL_DRONE = drone
listener("dji_sdk/tag_detections_for_simulator")
def main():
drone = DJIDrone()
drone.request_sdk_permission_control()
time.sleep(7)
#Motors running
drone.arm_drone()
time.sleep(5)
#Navigate the drone to a height of 5 meters
drone.local_position_navigation_send_request(0, 0, 5)
time.sleep(15)
#Delay for camera to save the image
time.sleep(15)
img = cv2.imread("/home/ubuntu/ck_ws/src/my_pkg/src/tutorial_package/mark.jpg", 0)
cv_image_gray = cv2.imread("/home/ubuntu/ck_ws/src/my_pkg/src/tutorial_package/capture1.jpg", 0)
x, y = test(img, cv_image_gray)
#Navigate the drone to the desired location of the target
if x!=0 and y!=0:
drone.local_position_navigation_send_request(x, 0, 3)
time.sleep(15)
drone.local_position_navigation_send_request(x, y, 3)
time.sleep(15)
#landing command
drone.landing()
time.sleep(20)
def main():
drone = DJIDrone()
display_main_menu()
while True:
main_operate_code = sys.stdin.read(1)
if main_operate_code == 'a':
drone.request_sdk_permission_control()
elif main_operate_code == 'b':
drone.release_sdk_permission_control()
elif main_operate_code == 'c':
drone.takeoff()
elif main_operate_code == 'd':
drone.landing()
elif main_operate_code == 'e':
drone.gohome()
elif main_operate_code == 'f':
drone.gimbal_angle_control(0, 0, 0, 20)
time.sleep(10)
elif main_operate_code == 'g':
drone.takeoff()
time.sleep(5)
for i in range(100):
if i < 90:
drone.attitude_control(0x40, 0, 2, 0, 0)
else:
drone.attitude_control(0x40, 0, 0, 0, 0)
time.sleep(0.02)
time.sleep(1)
for i in range(200):
if i < 180:
drone.attitude_control(0x40, 2, 0, 0, 0)
else:
drone.attitude_control(0x40, 0, 0, 0, 0)
time.sleep(0.02)
time.sleep(1)
for i in range(200):
if i < 180:
drone.attitude_control(0x40, -2, 0, 0, 0)
else:
drone.attitude_control(0x40, 0, 0, 0, 0)
time.sleep(0.02)
time.sleep(1)
for i in range(200):
if i < 180:
drone.attitude_control(0x40, 0, 2, 0, 0)
else:
drone.attitude_control(0x40, 0, 0, 0, 0)
time.sleep(0.02)
time.sleep(1)
for i in range(200):
if i < 180:
drone.attitude_control(0x40, 0, -2, 0, 0)
else:
drone.attitude_control(0x40, 0, 0, 0, 0)
time.sleep(0.02)
time.sleep(1)
for i in range(200):
if i < 180:
drone.attitude_control(0x40, 0, 0, 0.5, 0)
else:
drone.attitude_control(0x40, 0, 0, 0, 0)
time.sleep(0.02)
time.sleep(1)
for i in range(200):
if i < 180:
drone.attitude_control(0x40, 0, 0, -0.5, 0)
else:
drone.attitude_control(0x40, 0, 0, 0, 0)
time.sleep(0.02)
time.sleep(1)
for i in range(200):
if i < 180:
drone.attitude_control(0x40, 0, 0, 0, 90)
else:
drone.attitude_control(0x40, 0, 0, 0, 0)
time.sleep(0.02)
time.sleep(1)
for i in range(200):
if i < 180:
drone.attitude_control(0x40, 0, 0, 0, -90)
else:
drone.attitude_control(0x40, 0, 0, 0, 0)
time.sleep(0.02)
time.sleep(1)
drone.landing()
elif main_operate_code == 'h':
R = 2
V = 2
# start to draw circle
for i in range(300):
vx = V * math.sin((V / R) * i / 50.0)
vy = V * math.cos((V / R) * i / 50.0)
drone.attitude_control(
DJIDrone.HORIZ_POS | DJIDrone.VERT_VEL | DJIDrone.YAW_ANG
| DJIDrone.HORIZ_BODY | DJIDrone.STABLE_ON, vx, vy, 0, 0)
time.sleep(0.02)
elif main_operate_code == 'i':
# draw square sample
for i in range(60):
drone.attitude_control(
DJIDrone.HORIZ_POS | DJIDrone.VERT_VEL | DJIDrone.YAW_ANG
| DJIDrone.HORIZ_BODY | DJIDrone.STABLE_ON, 3, 3, 0, 0)
time.sleep(0.02)
for i in range(60):
drone.attitude_control(
DJIDrone.HORIZ_POS | DJIDrone.VERT_VEL | DJIDrone.YAW_ANG
| DJIDrone.HORIZ_BODY | DJIDrone.STABLE_ON, -3, 3, 0, 0)
time.sleep(0.02)
for i in range(60):
drone.attitude_control(
DJIDrone.HORIZ_POS | DJIDrone.VERT_VEL | DJIDrone.YAW_ANG
| DJIDrone.HORIZ_BODY | DJIDrone.STABLE_ON, -3, -3, 0, 0)
time.sleep(0.02)
for i in range(60):
drone.attitude_control(
DJIDrone.HORIZ_POS | DJIDrone.VERT_VEL | DJIDrone.YAW_ANG
| DJIDrone.HORIZ_BODY | DJIDrone.STABLE_ON, 3, -3, 0, 0)
time.sleep(0.02)
elif main_operate_code == 'j':
# take a picture
drone.take_picture()
elif main_operate_code == 'k':
# start video
drone.start_video()
elif main_operate_code == 'l':
# stop video
drone.stop_video()
elif main_operate_code == 'm':
# Local Navi Test
drone.local_position_navigation_send_request(-100, -100, 100)
elif main_operate_code == 'n':
# GPS Navi Test
drone.global_position_navigation_send_request(22.535, 113.95, 100)
elif main_operate_code == 'o':
# Waypoint List Navi Test
newWaypointList = [
dji_sdk.msg.Waypoint(latitude=22.535,
longitude=113.95,
altitude=100,
staytime=5,
heading=0),
dji_sdk.msg.Waypoint(latitude=22.535,
longitude=113.96,
altitude=100,
staytime=0,
heading=90),
dji_sdk.msg.Waypoint(latitude=22.545,
longitude=113.96,
altitude=100,
staytime=4,
heading=-90),
dji_sdk.msg.Waypoint(latitude=22.545,
longitude=113.96,
altitude=10,
staytime=2,
heading=180),
dji_sdk.msg.Waypoint(latitude=22.525,
longitude=113.93,
altitude=50,
staytime=0,
heading=-180)
]
drone.waypoint_navigation_send_request(newWaypointList)
elif main_operate_code == 'p':
drone.arm_drone()
elif main_operate_code == 'q':
drone.disarm_drone()
elif main_operate_code == 'r':
drone.vrc_start()
for i in range(100):
drone.vrc_control(1024 - 660, 1024 - 660, 1024 - 660,
1024 + 660)
time.sleep(0.02)
for i in range(1000):
drone.vrc_control(1024, 1024, 1024 + 660, 1024 + 660)
time.sleep(0.02)
for i in range(1000):
drone.vrc_control(1024 - 660, 1024 - 660)
time.sleep(0.02)
drone.vrc_stop()
elif main_operate_code == 's':
drone.sync_timestamp(50)
elif main_operate_code == 't':
return
else:
display_main_menu()
def init(self):
if sys.argv[1] == 'True':
self.use_dji_ = True
else:
self.use_dji_ = False
if self.use_dji_ == True:
self.drone_ = DJIDrone()
self.drone_.request_sdk_permission_control()
rospy.loginfo("Use DJI")
else:
rospy.init_node('circle_motion', anonymous=True)
self.odom_ = Odometry()
self.GLOBAL_FRAME_ = 0
self.LOCAL_FRAME_ = 1
self.target_xy_pos_ = np.zeros(2)
self.target_z_pos_ = 0.0
self.target_yaw_ = 0.0
self.target_frame_ = self.GLOBAL_FRAME_
self.initial_xy_pos_ = np.zeros(2)
self.initial_z_pos_ = 0.0
self.initial_yaw_ = 0.0
#state machine
self.INITIAL_STATE_ = 0
self.TAKEOFF_STATE_ = 1
self.TREE_DETECTION_START_STATE_ = 2
self.APPROACHING_TO_TREE_STATE_ = 3
self.START_CIRCLE_MOTION_STATE_ = 4
self.FINISH_CIRCLE_MOTION_STATE_ = 5
self.RETURN_HOME_STATE_ = 6
self.state_machine_ = self.INITIAL_STATE_
self.state_name_ = [
"initial", "takeoff", "tree detection start",
"approaching to tree", "circle motion", "finish circle motion",
"return home"
]
self.tree_detection_wait_count_ = 0
self.circle_motion_count_ = 0
self.odom_update_flag_ = False
self.uav_xy_pos_ = np.zeros(2)
self.uav_z_pos_ = 0.0
self.uav_yaw_ = 0.0
self.uav_yaw_old_ = 0.0
self.uav_yaw_overflow_ = 0
self.uav_accumulated_yaw_ = 0.0
self.cycle_count_ = 0
self.tree_xy_pos_ = np.zeros(2)
self.task_start_ = False
self.vel_pub_topic_name_ = rospy.get_param("~vel_pub_topic_name",
"cmd_vel")
self.state_machine_pub_topic_name_ = rospy.get_param(
"~state_machine_pub_topic_name", "state_machine")
self.target_pos_pub_topic_name_ = rospy.get_param(
"~target_pos_pub_topic_name", "uav_target_pos")
self.uav_odom_sub_topic_name_ = rospy.get_param(
"~uav_odom_sub_topic_name", "ground_truth/state")
self.tree_location_sub_topic_name_ = rospy.get_param(
"~tree_location_sub_topic_name", "tree_location")
self.tree_detection_start_pub_topic_name_ = rospy.get_param(
"~tree_detection_start_pub_topic_name", "detection_start")
self.task_start_service_name_ = rospy.get_param(
"~task_start_service_name", "task_start")
self.control_rate_ = rospy.get_param("~control_rate", 20)
self.takeoff_height_ = rospy.get_param("~takeoff_height", 1.5)
self.nav_xy_pos_pgain_ = rospy.get_param("~nav_xy_pos_pgain", 1.0)
self.nav_z_pos_pgain_ = rospy.get_param("~nav_z_pos_pgain", 1.0)
self.nav_yaw_pgain_ = rospy.get_param("~nav_yaw_pgain", 1.0)
self.nav_xy_vel_thresh_ = rospy.get_param("~nav_xy_vel_thresh", 2.0)
self.nav_z_vel_thresh_ = rospy.get_param("~nav_z_vel_thresh", 2.0)
self.nav_yaw_vel_thresh_ = rospy.get_param("~nav_yaw_vel_thresh", 1.0)
self.nav_pos_convergence_thresh_ = rospy.get_param(
"~nav_pos_convergence_thresh", 0.1)
self.nav_yaw_convergence_thresh_ = rospy.get_param(
"~nav_yaw_convergence_thresh", 0.05)
self.nav_vel_convergence_thresh_ = rospy.get_param(
"~nav_vel_convergence_thresh", 0.1)
self.circle_radius_ = rospy.get_param("~circle_radius", 1.0)
self.circle_y_vel_ = rospy.get_param("~circle_y_vel", 0.5)
self.circle_motion_times_ = rospy.get_param("~circle_motion_times", 1)
self.circle_motion_height_step_ = rospy.get_param(
"~circle_motion_height_step_", 0.5)
self.tree_detection_wait_ = rospy.get_param("~tree_detection_wait",
1.0)
self.task_kind_ = rospy.get_param("~task_kind",
1) #1 yosen 2 honsen 3 kesshou
self.control_timer_ = rospy.Timer(
rospy.Duration(1.0 / self.control_rate_), self.controlCallback)
self.vel_pub_ = rospy.Publisher(self.vel_pub_topic_name_,
Twist,
queue_size=10)
self.state_machine_pub_ = rospy.Publisher(
self.state_machine_pub_topic_name_, String, queue_size=10)
self.target_pos_pub_ = rospy.Publisher(self.target_pos_pub_topic_name_,
Odometry,
queue_size=10)
self.tree_detection_start_pub_ = rospy.Publisher(
self.tree_detection_start_pub_topic_name_, Bool, queue_size=10)
self.odom_sub_ = rospy.Subscriber(self.uav_odom_sub_topic_name_,
Odometry, self.odomCallback)
self.tree_location_sub_ = rospy.Subscriber(
self.tree_location_sub_topic_name_, PointStamped,
self.treeLocationCallback)
self.task_start_service_ = rospy.Service(self.task_start_service_name_,
SetBool,
self.taskStartCallback)
class CircleMotion:
def init(self):
if sys.argv[1] == 'True':
self.use_dji_ = True
else:
self.use_dji_ = False
if self.use_dji_ == True:
self.drone_ = DJIDrone()
self.drone_.request_sdk_permission_control()
rospy.loginfo("Use DJI")
else:
rospy.init_node('circle_motion', anonymous=True)
self.odom_ = Odometry()
self.GLOBAL_FRAME_ = 0
self.LOCAL_FRAME_ = 1
self.target_xy_pos_ = np.zeros(2)
self.target_z_pos_ = 0.0
self.target_yaw_ = 0.0
self.target_frame_ = self.GLOBAL_FRAME_
self.initial_xy_pos_ = np.zeros(2)
self.initial_z_pos_ = 0.0
self.initial_yaw_ = 0.0
#state machine
self.INITIAL_STATE_ = 0
self.TAKEOFF_STATE_ = 1
self.TREE_DETECTION_START_STATE_ = 2
self.APPROACHING_TO_TREE_STATE_ = 3
self.START_CIRCLE_MOTION_STATE_ = 4
self.FINISH_CIRCLE_MOTION_STATE_ = 5
self.RETURN_HOME_STATE_ = 6
self.state_machine_ = self.INITIAL_STATE_
self.state_name_ = [
"initial", "takeoff", "tree detection start",
"approaching to tree", "circle motion", "finish circle motion",
"return home"
]
self.tree_detection_wait_count_ = 0
self.circle_motion_count_ = 0
self.odom_update_flag_ = False
self.uav_xy_pos_ = np.zeros(2)
self.uav_z_pos_ = 0.0
self.uav_yaw_ = 0.0
self.uav_yaw_old_ = 0.0
self.uav_yaw_overflow_ = 0
self.uav_accumulated_yaw_ = 0.0
self.cycle_count_ = 0
self.tree_xy_pos_ = np.zeros(2)
self.task_start_ = False
self.vel_pub_topic_name_ = rospy.get_param("~vel_pub_topic_name",
"cmd_vel")
self.state_machine_pub_topic_name_ = rospy.get_param(
"~state_machine_pub_topic_name", "state_machine")
self.target_pos_pub_topic_name_ = rospy.get_param(
"~target_pos_pub_topic_name", "uav_target_pos")
self.uav_odom_sub_topic_name_ = rospy.get_param(
"~uav_odom_sub_topic_name", "ground_truth/state")
self.tree_location_sub_topic_name_ = rospy.get_param(
"~tree_location_sub_topic_name", "tree_location")
self.tree_detection_start_pub_topic_name_ = rospy.get_param(
"~tree_detection_start_pub_topic_name", "detection_start")
self.task_start_service_name_ = rospy.get_param(
"~task_start_service_name", "task_start")
self.control_rate_ = rospy.get_param("~control_rate", 20)
self.takeoff_height_ = rospy.get_param("~takeoff_height", 1.5)
self.nav_xy_pos_pgain_ = rospy.get_param("~nav_xy_pos_pgain", 1.0)
self.nav_z_pos_pgain_ = rospy.get_param("~nav_z_pos_pgain", 1.0)
self.nav_yaw_pgain_ = rospy.get_param("~nav_yaw_pgain", 1.0)
self.nav_xy_vel_thresh_ = rospy.get_param("~nav_xy_vel_thresh", 2.0)
self.nav_z_vel_thresh_ = rospy.get_param("~nav_z_vel_thresh", 2.0)
self.nav_yaw_vel_thresh_ = rospy.get_param("~nav_yaw_vel_thresh", 1.0)
self.nav_pos_convergence_thresh_ = rospy.get_param(
"~nav_pos_convergence_thresh", 0.1)
self.nav_yaw_convergence_thresh_ = rospy.get_param(
"~nav_yaw_convergence_thresh", 0.05)
self.nav_vel_convergence_thresh_ = rospy.get_param(
"~nav_vel_convergence_thresh", 0.1)
self.circle_radius_ = rospy.get_param("~circle_radius", 1.0)
self.circle_y_vel_ = rospy.get_param("~circle_y_vel", 0.5)
self.circle_motion_times_ = rospy.get_param("~circle_motion_times", 1)
self.circle_motion_height_step_ = rospy.get_param(
"~circle_motion_height_step_", 0.5)
self.tree_detection_wait_ = rospy.get_param("~tree_detection_wait",
1.0)
self.task_kind_ = rospy.get_param("~task_kind",
1) #1 yosen 2 honsen 3 kesshou
self.control_timer_ = rospy.Timer(
rospy.Duration(1.0 / self.control_rate_), self.controlCallback)
self.vel_pub_ = rospy.Publisher(self.vel_pub_topic_name_,
Twist,
queue_size=10)
self.state_machine_pub_ = rospy.Publisher(
self.state_machine_pub_topic_name_, String, queue_size=10)
self.target_pos_pub_ = rospy.Publisher(self.target_pos_pub_topic_name_,
Odometry,
queue_size=10)
self.tree_detection_start_pub_ = rospy.Publisher(
self.tree_detection_start_pub_topic_name_, Bool, queue_size=10)
self.odom_sub_ = rospy.Subscriber(self.uav_odom_sub_topic_name_,
Odometry, self.odomCallback)
self.tree_location_sub_ = rospy.Subscriber(
self.tree_location_sub_topic_name_, PointStamped,
self.treeLocationCallback)
self.task_start_service_ = rospy.Service(self.task_start_service_name_,
SetBool,
self.taskStartCallback)
def odomCallback(self, msg):
self.odom_ = msg
self.uav_xy_pos_ = np.array(
[msg.pose.pose.position.x, msg.pose.pose.position.y])
self.uav_z_pos_ = msg.pose.pose.position.z
quaternion = np.array([
msg.pose.pose.orientation.x, msg.pose.pose.orientation.y,
msg.pose.pose.orientation.z, msg.pose.pose.orientation.w
])
self.uav_yaw_ = tf.transformations.euler_from_quaternion(quaternion)[2]
if self.odom_update_flag_ == True:
if self.uav_yaw_ - self.uav_yaw_old_ > 5.0:
self.uav_yaw_overflow_ -= 1
elif self.uav_yaw_old_ - self.uav_yaw_old_ < -5.0:
self.uav_yaw_overflow_ += 1
self.uav_accumulated_yaw_ = self.uav_yaw_ + 2 * math.pi * self.uav_yaw_overflow_
self.uav_yaw_old_ = self.uav_yaw_
self.odom_update_flag_ = True
def treeDetectionStart(self):
msg = Bool()
msg.data = True
self.tree_detection_start_pub_.publish(msg)
def treeLocationCallback(self, msg):
self.tree_xy_pos_ = np.array([msg.point.x, msg.point.y])
def taskStartCallback(self, req):
res = SetBoolResponse()
if req.data == True:
self.task_start_ = True
res.success = True
res.message = "Task Start"
rospy.loginfo("Task Start")
else:
self.task_start_ = False
res.success = False
res.message = "Task does not start"
rospy.loginfo("Task does not start")
return res
def isConvergent(self, frame, target_xy_pos, target_z_pos, target_yaw):
if frame == self.GLOBAL_FRAME_:
delta_pos = np.array([
target_xy_pos[0] - self.uav_xy_pos_[0],
target_xy_pos[1] - self.uav_xy_pos_[1],
target_z_pos - self.uav_z_pos_
])
elif frame == self.LOCAL_FRAME_:
delta_pos = np.array([
target_xy_pos[0], target_xy_pos[1],
target_z_pos - self.uav_z_pos_
])
else:
return
delta_yaw = target_yaw - self.uav_yaw_
current_vel = np.array([
self.odom_.twist.twist.linear.x, self.odom_.twist.twist.linear.y,
self.odom_.twist.twist.linear.z
])
if np.linalg.norm(
delta_pos) < self.nav_pos_convergence_thresh_ and abs(
delta_yaw
) < self.nav_yaw_convergence_thresh_ and np.linalg.norm(
current_vel) < self.nav_vel_convergence_thresh_:
return True
else:
return False
def saturateVelocity(self, xy_vel, z_vel, yaw_vel):
ret = [xy_vel, z_vel, yaw_vel]
if np.linalg.norm(xy_vel) > self.nav_xy_vel_thresh_:
ret[0] = xy_vel * (self.nav_xy_vel_thresh_ /
np.linalg.norm(xy_vel))
if abs(z_vel) > self.nav_z_vel_thresh_:
ret[1] = z_vel * self.nav_z_vel_thresh_ / abs(z_vel)
if abs(yaw_vel) > self.nav_yaw_vel_thresh_:
ret[2] = yaw_vel * self.nav_yaw_vel_thresh_ / abs(yaw_vel)
return ret
#go pos function: z and yaw are always in global frame
def goPos(self, frame, target_xy, target_z, target_yaw):
if frame == self.GLOBAL_FRAME_:
rot_mat = np.array(
[[math.cos(self.uav_yaw_),
math.sin(self.uav_yaw_)],
[-math.sin(self.uav_yaw_),
math.cos(self.uav_yaw_)]])
delta_xy = np.dot(rot_mat, target_xy - self.uav_xy_pos_)
elif frame == self.LOCAL_FRAME_:
delta_xy = target_xy
else:
return
delta_z = target_z - self.uav_z_pos_
delta_yaw = target_yaw - self.uav_yaw_
if delta_yaw > math.pi:
delta_yaw -= math.pi * 2
elif delta_yaw < -math.pi:
delta_yaw += math.pi * 2
nav_xy_vel = delta_xy * self.nav_xy_pos_pgain_
nav_z_vel = delta_z * self.nav_z_pos_pgain_
nav_yaw_vel = delta_yaw * self.nav_yaw_pgain_
nav_xy_vel, nav_z_vel, nav_yaw_vel = self.saturateVelocity(
nav_xy_vel, nav_z_vel, nav_yaw_vel)
vel_msg = Twist()
vel_msg.linear.x = nav_xy_vel[0]
vel_msg.linear.y = nav_xy_vel[1]
vel_msg.linear.z = nav_z_vel
vel_msg.angular.z = nav_yaw_vel
#for debug
self.target_xy_pos_ = target_xy
self.target_z_pos_ = target_z
self.target_yaw_ = target_yaw
self.target_frame_ = frame
return vel_msg
def goCircle(self, circle_center_xy, target_z, target_y_vel, radius):
nav_xy_vel = np.zeros(2)
nav_xy_vel[0] = -(radius -
circle_center_xy[0]) * self.nav_xy_pos_pgain_
nav_xy_vel[1] = target_y_vel
nav_z_vel = target_z - self.uav_z_pos_
nav_yaw_vel = math.atan2(
circle_center_xy[1], circle_center_xy[0]
) * self.nav_yaw_pgain_ - target_y_vel / radius #feedback+feedforward
nav_xy_vel, nav_z_vel, nav_yaw_vel = self.saturateVelocity(
nav_xy_vel, nav_z_vel, nav_yaw_vel)
vel_msg = Twist()
vel_msg.linear.x = nav_xy_vel[0]
vel_msg.linear.y = nav_xy_vel[1]
vel_msg.linear.z = nav_z_vel
vel_msg.angular.z = nav_yaw_vel
return vel_msg
def waitCycle(self, cycle, reset):
if reset == 'True':
self.cycle_count_ = 0
return False
else:
self.cycle_count_ += 1
if self.cycle_count_ >= cycle:
return True
else:
return False
def controlCallback(self, event):
if (not self.odom_update_flag_) or (not self.task_start_):
return
#navigation
vel_msg = Twist()
if self.state_machine_ == self.INITIAL_STATE_:
vel_msg.linear.x = vel_msg.linear.y = vel_msg.linear.z = vel_msg.angular.z = 0.0
if self.state_machine_ == self.TAKEOFF_STATE_ or self.state_machine_ == self.TREE_DETECTION_START_STATE_:
vel_msg = self.goPos(self.GLOBAL_FRAME_, self.initial_xy_pos_,
self.takeoff_height_,
self.initial_yaw_) #hover
if self.state_machine_ == self.APPROACHING_TO_TREE_STATE_:
tree_direction = math.atan2(self.tree_xy_pos_[1],
self.tree_xy_pos_[0])
vel_msg = self.goPos(
self.LOCAL_FRAME_,
np.array([
self.tree_xy_pos_[0] -
self.circle_radius_ * math.cos(tree_direction),
self.tree_xy_pos_[1] -
self.circle_radius_ * math.sin(tree_direction)
]), self.takeoff_height_, self.uav_yaw_ + tree_direction)
if self.state_machine_ == self.START_CIRCLE_MOTION_STATE_:
vel_msg = self.goCircle(
self.tree_xy_pos_, self.takeoff_height_ +
self.circle_motion_count_ * self.circle_motion_height_step_,
self.circle_y_vel_, self.circle_radius_)
if self.state_machine_ == self.FINISH_CIRCLE_MOTION_STATE_:
#use global frame
#vel_msg = self.goPos(self.GLOBAL_FRAME_, self.circle_initial_xy_, self.takeoff_height_, self.circle_initial_yaw_)
#use local frame
tree_direction = math.atan2(self.tree_xy_pos_[1],
self.tree_xy_pos_[0])
vel_msg = self.goPos(
self.LOCAL_FRAME_,
np.array([
self.tree_xy_pos_[0] -
self.circle_radius_ * math.cos(tree_direction),
self.tree_xy_pos_[1] -
self.circle_radius_ * math.sin(tree_direction)
]), self.takeoff_height_, self.uav_yaw_ + tree_direction)
if self.state_machine_ == self.RETURN_HOME_STATE_:
#use global frame
#vel_msg = self.goPos(self.GLOBAL_FRAME_, self.initial_xy_pos_, self.takeoff_height_, self.initial_yaw_)
#use local frame
vel_msg = self.goPos(
self.LOCAL_FRAME_,
self.tree_xy_pos_ - self.initial_target_tree_xy_pos_,
self.takeoff_height_, self.initial_yaw_)
#end navigation
#state machine
if self.state_machine_ == self.INITIAL_STATE_:
self.initial_xy_pos_ = np.array(self.uav_xy_pos_)
self.initial_yaw_ = self.uav_yaw_
if self.use_dji_ == True:
self.drone_.takeoff()
self.state_machine_ = self.TAKEOFF_STATE_
rospy.loginfo("take off")
elif self.state_machine_ == self.TAKEOFF_STATE_:
if self.isConvergent(self.target_frame_, self.target_xy_pos_,
self.target_z_pos_, self.target_yaw_):
self.treeDetectionStart()
self.state_machine_ = self.TREE_DETECTION_START_STATE_
elif self.state_machine_ == self.TREE_DETECTION_START_STATE_:
self.tree_detection_wait_count_ += 1
if self.tree_detection_wait_count_ > self.control_rate_ * self.tree_detection_wait_:
self.state_machine_ = self.APPROACHING_TO_TREE_STATE_
self.initial_target_tree_xy_pos_ = np.array(self.tree_xy_pos_)
elif self.state_machine_ == self.APPROACHING_TO_TREE_STATE_:
if self.isConvergent(self.target_frame_, self.target_xy_pos_,
self.target_z_pos_, self.target_yaw_):
if self.task_kind_ == 1:
self.state_machine_ = self.RETURN_HOME_STATE_
elif self.task_kind_ == 2:
self.state_machine_ = self.START_CIRCLE_MOTION_STATE_
self.circle_initial_yaw_ = self.uav_accumulated_yaw_
self.circle_initial_xy_ = np.array(self.uav_xy_pos_)
elif self.state_machine_ == self.START_CIRCLE_MOTION_STATE_:
if abs(self.circle_initial_yaw_ -
self.uav_accumulated_yaw_) > 2 * math.pi:
self.circle_motion_count_ += 1
self.circle_initial_yaw_ = self.uav_accumulated_yaw_
if self.circle_motion_count_ == self.circle_motion_times_:
self.state_machine_ = self.FINISH_CIRCLE_MOTION_STATE_
elif self.state_machine_ == self.FINISH_CIRCLE_MOTION_STATE_:
if self.isConvergent(self.target_frame_, self.target_xy_pos_,
self.target_z_pos_, self.target_yaw_):
self.state_machine_ = self.RETURN_HOME_STATE_
elif self.state_machine_ == self.RETURN_HOME_STATE_:
pass
#end state machine
#publication
self.state_machine_pub_.publish(self.state_name_[self.state_machine_])
target_pos_msg = Odometry()
target_pos_msg.header = self.odom_.header
if self.target_frame_ == self.GLOBAL_FRAME_:
target_pos_msg.child_frame_id = "global"
elif self.target_frame_ == self.LOCAL_FRAME_:
target_pos_msg.child_frame_id = "local"
target_pos_msg.pose.pose.position.x = self.target_xy_pos_[0]
target_pos_msg.pose.pose.position.y = self.target_xy_pos_[1]
target_pos_msg.pose.pose.position.z = self.target_z_pos_
target_pos_msg.pose.pose.orientation.w = self.target_yaw_
self.target_pos_pub_.publish(target_pos_msg)
self.vel_pub_.publish(vel_msg)
if self.use_dji_ == True:
self.drone_.velocity_control(0, vel_msg.linear.x,
-vel_msg.linear.y, vel_msg.linear.z,
-vel_msg.angular.z * 180 /
math.pi) #machine frame yaw_rate[deg]
def main():
getch = _find_getch()
drone = DJIDrone()
display_main_menu()
while True:
ch = getch()
if ch == 'c':
drone.request_sdk_permission_control()
drone.takeoff()
time.sleep(1.0)
elif ch == 'w':
drone.vrc_start()
drone.vrc_control(1024 + 110, 1024, 1024, 1024)
time.sleep(0.6)
drone.vrc_stop()
elif ch == 's':
drone.vrc_start()
drone.vrc_control(1024 - 110, 1024, 1024, 1024)
time.sleep(0.6)
drone.vrc_stop()
elif ch == 'a':
drone.vrc_start()
drone.vrc_control(1024, 1024 + 110, 1024, 1024)
time.sleep(0.6)
drone.vrc_stop()
elif ch == 'd':
drone.vrc_start()
drone.vrc_control(1024, 1024 - 110, 1024, 1024)
time.sleep(0.6)
drone.vrc_stop()
elif ch == 'i':
drone.vrc_start()
drone.vrc_control(1024, 1024, 1024 + 110, 1024)
time.sleep(0.6)
drone.vrc_stop()
elif ch == 'k':
drone.vrc_start()
drone.vrc_control(1024, 1024, 1024 - 110, 1024)
time.sleep(0.6)
drone.vrc_stop()
elif ch == 'j':
drone.vrc_start()
drone.vrc_control(1024, 1024, 1024, 1024 - 110)
time.sleep(0.6)
drone.vrc_stop()
elif ch == 'l':
drone.vrc_start()
drone.vrc_control(1024, 1024, 1024, 1024 + 110)
time.sleep(0.6)
drone.vrc_stop()
elif ch == 'v':
#drone.release_sdk_permission_control()
break
drone.landing()
"""
error_yaw = abs(self.carrot.t - self.cLoc.t)
if (err < 1.0 and error_yaw < 8.0):
return True
else:
return False
def quaternions_to_RPY(self, q):
yaw = math.atan2(2.0 * (q[3] * q[2] + q[0] * q[1]),
1.0 - 2.0 * (q[1] * q[1] + q[2] * q[2]))
#pitch = math.asin(2.0 * (q[2] * q[0] - q[3] * q[1]) )
#roll = math.atan2(2.0 * (q[3] * q[0] + q[1] * q[2]) , - 1.0 + 2.0 * (q[0] * q[0] + q[1] * q[1]) )
return yaw
if __name__ == "__main__":
drone = DJIDrone()
pid = PID_Controller()
ai = rospy.get_param('/agent_index')
aa = rospy.get_param('/agent_altitudes')
cs = rospy.get_param('/cruising_speeds')
sp = rospy.get_param('/speed_penalty')
cs = cs[ai]
aa = aa[ai]
rospy.loginfo("Quad PID Controller::initializing")
rospy.loginfo(" Quad PID Controller::agent index: %i", ai)
rospy.loginfo(" Quad PID Controller::agent altitude: %.1f", aa)
rospy.loginfo(" Quad PID Controller::cruising speed: %.1f", cs)
rospy.loginfo(" Quad PID Controller::speed penalty: %.1f", sp)
np.savetxt(f, data, fmt='%s')
f.close()
if __name__ == "__main__":
now = datetime.datetime.now()
#The file that is written to is created here. Its name is just the date. If you want a different file for
#different times/sessions in the same day, you will have to change that.
#The path to the file is also from root, because this file needs to be run with sudo (for reasons explained further in the README. Hint: It's the Atmotube.)
#f = open('catkin_ws/src/Onboard-SDK-ROS-3.2/dji_sdk_demo/script/Atmotube/data/' + now.strftime("%Y-%m-%d") + '.dat', 'a+')
f = open(
'catkin_ws/src/Onboard-SDK-ROS-3.2/dji_sdk_demo/script/Atmotube/data/sample.dat',
'a+')
f.close()
#Make the plotter thread
plotter = gaussiantest.Plotter()
plotter_thread = threading.Thread(target=plotter.run)
plotter_thread.start()
#Make the atmotube thread
atmotube_thread = threading.Thread(target=get_data)
atmotube_thread.daemon = True
atmotube_thread.start()
drone = DJIDrone()
drone.request_sdk_permission_control()
listener()
def main():
drone = DJIDrone()
display_main_menu()
while True:
main_operate_code = sys.stdin.read(1)
if main_operate_code == 'a':
drone.request_sdk_permission_control()
elif main_operate_code == 'b':
drone.release_sdk_permission_control()
elif main_operate_code == 'c':
drone.takeoff()
elif main_operate_code == 'd':
drone.landing()
elif main_operate_code =='e':
drone.gohome()
elif main_operate_code == 'f':
drone.gimbal_angle_control(0, 0, 0, 20)
time.sleep(2)
drone.gimbal_angle_control(0, 0, 1800, 20)
time.sleep(2)
drone.gimbal_angle_control(0, 0, -1800, 20)
time.sleep(2)
drone.gimbal_angle_control(300, 0, 0, 20)
time.sleep(2)
drone.gimbal_angle_control(-300, 0, 0, 20)
time.sleep(2)
drone.gimbal_angle_control(0, 300, 0, 20)
time.sleep(2)
drone.gimbal_angle_control(0, -300, 0, 20)
time.sleep(2)
drone.gimbal_speed_control(100, 0, 0)
time.sleep(2)
drone.gimbal_speed_control(-100, 0, 0)
time.sleep(2)
drone.gimbal_speed_control(0, 0, 200)
time.sleep(2)
drone.gimbal_speed_control(0, 0, -200)
time.sleep(2)
drone.gimbal_speed_control(0, 200, 0)
time.sleep(2)
drone.gimbal_speed_control(0, -200, 0)
time.sleep(2)
drone.gimbal_angle_control(0, 0, 0, 20)
elif main_operate_code == 'g':
drone.takeoff()
time.sleep(5)
for i in range(100):
if i < 90:
drone.attitude_control(0x40, 0, 2, 0, 0)
else:
drone.attitude_control(0x40, 0, 0, 0, 0)
time.sleep(0.02)
time.sleep(1)
for i in range(200):
if i < 180:
drone.attitude_control(0x40, 2, 0, 0, 0)
else:
drone.attitude_control(0x40, 0, 0, 0, 0)
time.sleep(0.02)
time.sleep(1)
for i in range(200):
if i < 180:
drone.attitude_control(0x40, -2, 0, 0, 0)
else:
drone.attitude_control(0x40, 0, 0, 0, 0)
time.sleep(0.02)
time.sleep(1)
for i in range(200):
if i < 180:
drone.attitude_control(0x40, 0, 2, 0, 0)
else:
drone.attitude_control(0x40, 0, 0, 0, 0)
time.sleep(0.02)
time.sleep(1)
for i in range(200):
if i < 180:
drone.attitude_control(0x40, 0, -2, 0, 0)
else:
drone.attitude_control(0x40, 0, 0, 0, 0)
time.sleep(0.02)
time.sleep(1)
for i in range(200):
if i < 180:
drone.attitude_control(0x40, 0, 0, 0.5, 0)
else:
drone.attitude_control(0x40, 0, 0, 0, 0)
time.sleep(0.02)
time.sleep(1)
for i in range(200):
if i < 180:
drone.attitude_control(0x40, 0, 0, -0.5, 0)
else:
drone.attitude_control(0x40, 0, 0, 0, 0)
time.sleep(0.02)
time.sleep(1)
for i in range(200):
if i < 180:
drone.attitude_control(0x40, 0, 0, 0, 90)
else:
drone.attitude_control(0x40, 0, 0, 0, 0)
time.sleep(0.02)
time.sleep(1)
for i in range(200):
if i < 180:
drone.attitude_control(0x40, 0, 0, 0, -90)
else:
drone.attitude_control(0x40, 0, 0, 0, 0)
time.sleep(0.02)
time.sleep(1)
drone.landing()
elif main_operate_code == 'h':
R = 2
V = 2
# start to draw circle
for i in range(300):
vx = V * math.sin((V/R)*i/50.0)
vy = V * math.cos((V/R)*i/50.0)
drone.attitude_control(DJIDrone.HORIZ_POS|DJIDrone.VERT_VEL|DJIDrone.YAW_ANG|DJIDrone.HORIZ_BODY|DJIDrone.STABLE_ON, vx, vy, 0, 0)
time.sleep(0.02)
elif main_operate_code == 'i':
# draw square sample
for i in range(60):
drone.attitude_control(DJIDrone.HORIZ_POS|DJIDrone.VERT_VEL|DJIDrone.YAW_ANG|DJIDrone.HORIZ_BODY|DJIDrone.STABLE_ON, 3, 3, 0, 0)
time.sleep(0.02)
for i in range(60):
drone.attitude_control(DJIDrone.HORIZ_POS|DJIDrone.VERT_VEL|DJIDrone.YAW_ANG|DJIDrone.HORIZ_BODY|DJIDrone.STABLE_ON, -3, 3, 0, 0)
time.sleep(0.02)
for i in range(60):
drone.attitude_control(DJIDrone.HORIZ_POS|DJIDrone.VERT_VEL|DJIDrone.YAW_ANG|DJIDrone.HORIZ_BODY|DJIDrone.STABLE_ON, -3, -3, 0, 0)
time.sleep(0.02)
for i in range(60):
drone.attitude_control(DJIDrone.HORIZ_POS|DJIDrone.VERT_VEL|DJIDrone.YAW_ANG|DJIDrone.HORIZ_BODY|DJIDrone.STABLE_ON, 3, -3, 0, 0)
time.sleep(0.02)
elif main_operate_code == 'j':
# take a picture
drone.take_picture()
elif main_operate_code == 'k':
# start video
drone.start_video()
elif main_operate_code == 'l':
# stop video
drone.stop_video()
elif main_operate_code == 'm':
# Local Navi Test
drone.local_position_navigation_send_request(-100, -100, 100)
elif main_operate_code == 'n':
# GPS Navi Test
drone.global_position_navigation_send_request(22.535, 113.95, 100)
elif main_operate_code == 'o':
# Waypoint List Navi Test
newWaypointList = [
dji_sdk.msg.Waypoint(latitude = 22.535, longitude = 113.95, altitude = 100, staytime = 5, heading = 0),
dji_sdk.msg.Waypoint(latitude = 22.535, longitude = 113.96, altitude = 100, staytime = 0, heading = 90),
dji_sdk.msg.Waypoint(latitude = 22.545, longitude = 113.96, altitude = 100, staytime = 4, heading = -90),
dji_sdk.msg.Waypoint(latitude = 22.545, longitude = 113.96, altitude = 10, staytime = 2, heading = 180),
dji_sdk.msg.Waypoint(latitude = 22.525, longitude = 113.93, altitude = 50, staytime = 0, heading = -180)]
drone.waypoint_navigation_send_request(newWaypointList)
elif main_operate_code == 'p':
drone.arm_drone()
elif main_operate_code == 'q':
drone.disarm_drone()
elif main_operate_code == 'r':
drone.vrc_start();
for i in range(100):
drone.vrc_control(1024-660, 1024-660, 1024-660, 1024+660)
time.sleep(0.02)
for i in range(1000):
drone.vrc_control(1024, 1024, 1024+660, 1024+660)
time.sleep(0.02)
for i in range(1000):
drone.vrc_control(1024-660, 1024-660)
time.sleep(0.02)
drone.vrc_stop()
elif main_operate_code == 's':
drone.sync_timestamp(50)
elif main_operate_code == 't':
return
else:
display_main_menu()
rospy.Subscriber('dji_sdk/local_position', dji_sdk.msg.LocalPosition, callback)
rospy.spin()
def BinarySearch(pos):
global L,R, current_pos, SetPoint
if(pos==theta):
while(abs(current_pos) > 0.5):
print('current pos :%f'%current_pos)
drone.local_position_navigation_send_request(0,0,Height)
drone.gohome()
return 0
elif(pos>theta):
R=SetPoint
return (L+R)/2
elif(pos<theta):
L=SetPoint
return (L+R)/2
if __name__ == '__main__':
global SetPoint
SetPoint=BinarySearch(current_pos)
#SetPoint=25.0
global Height
print(SetPoint)
drone = DJIDrone()
drone.request_sdk_permission_control()
drone.arm_drone()
drone.local_position_navigation_send_request(SetPoint,0,Height)
listener()
def init(self):
if sys.argv[1] == 'True':
self.use_dji_ = True
else:
self.use_dji_ = False
if self.use_dji_ == True:
self.drone_ = DJIDrone()
self.drone_.request_sdk_permission_control()
rospy.loginfo("Use DJI")
else:
rospy.init_node('circle_motion', anonymous=True)
self.odom_ = Odometry()
self.GLOBAL_FRAME_ = 0
self.LOCAL_FRAME_ = 1
self.target_xy_pos_ = np.zeros(2)
self.target_z_pos_ = 0.0
self.target_yaw_ = 0.0
self.target_frame_ = self.GLOBAL_FRAME_
self.initial_xy_global_pos_ = np.zeros(2)
self.initial_z_pos_ = 0.0
self.initial_yaw_ = 0.0
#state machine
self.INITIAL_STATE_ = 0
self.TAKEOFF_STATE_ = 1
self.TREE_DETECTION_START_STATE_ = 2
self.APPROACHING_TO_TREE_STATE_ = 3
self.START_CIRCLE_MOTION_STATE_ = 4
self.FINISH_CIRCLE_MOTION_STATE_ = 5
self.TURN_STATE_ = 6
self.RETURN_HOME_STATE_ = 7
self.FINISH_STATE_ = 8
self.state_machine_ = self.INITIAL_STATE_
self.state_name_ = [
"initial", "takeoff", "tree detection start",
"approaching to tree", "circle motion", "finish circle motion",
"turn", "return home", "finish"
]
self.circle_motion_count_ = 0
self.target_count_ = 0
self.odom_update_flag_ = False
self.uav_xy_global_pos_ = np.zeros(2)
self.uav_z_pos_ = 0.0
self.uav_yaw_ = 0.0
self.uav_yaw_old_ = 0.0
self.uav_yaw_overflow_ = 0
self.uav_accumulated_yaw_ = 0.0
self.tree_cluster_ = LaserScan()
self.tree_xy_local_pos_ = np.zeros(2)
self.tree_pos_update_flag_ = False
self.task_start_ = False
self.task_start_time_ = rospy.Time.now()
self.task_elapsed_time_ = rospy.Time(0)
self.turn_uav_xy_global_pos_ = np.zeros(2)
self.vel_pub_topic_name_ = rospy.get_param("~vel_pub_topic_name",
"cmd_vel")
self.state_machine_pub_topic_name_ = rospy.get_param(
"~state_machine_pub_topic_name", "state_machine")
self.target_pos_pub_topic_name_ = rospy.get_param(
"~target_pos_pub_topic_name", "uav_target_pos")
self.tracking_control_pub_topic_name_ = rospy.get_param(
"~tracking_control_pub_topic_name", "/tracking_control")
self.state_visualization_pub_topic_name_ = rospy.get_param(
"state_visualization_pub_topic_name", "overlay_text")
self.task_start_sub_topic_name_ = rospy.get_param(
"~task_start_sub_topic_name", "task_start")
self.uav_odom_sub_topic_name_ = rospy.get_param(
"~uav_odom_sub_topic_name", "ground_truth/state")
self.tree_location_sub_topic_name_ = rospy.get_param(
"~tree_location_sub_topic_name", "tree_location")
self.tree_detection_start_pub_topic_name_ = rospy.get_param(
"~tree_detection_start_pub_topic_name", "detection_start")
self.tree_cluster_sub_topic_name_ = rospy.get_param(
"~tree_cluster_sub_topic_name", "scan_clustered")
self.update_target_tree_service_name_ = rospy.get_param(
"~update_target_tree_service_name", "/update_target_tree")
self.global_state_name_sub_topic_name_ = rospy.get_param(
"~global_state_name_sub_topic_name", "state_machine")
self.set_first_tree_service_name_ = rospy.get_param(
"~set_first_tree_service_name_", "/set_first_tree")
self.control_rate_ = rospy.get_param("~control_rate", 20)
#navigation
self.takeoff_height_ = rospy.get_param("~takeoff_height", 1.5)
self.return_height_ = rospy.get_param("~return_height", 1.5)
self.nav_xy_pos_pgain_ = rospy.get_param("~nav_xy_pos_pgain", 1.0)
self.nav_z_pos_pgain_ = rospy.get_param("~nav_z_pos_pgain", 1.0)
self.nav_yaw_pgain_ = rospy.get_param("~nav_yaw_pgain", 1.0)
self.nav_xy_vel_thresh_ = rospy.get_param("~nav_xy_vel_thresh", 2.0)
self.nav_z_vel_thresh_ = rospy.get_param("~nav_z_vel_thresh", 2.0)
self.nav_yaw_vel_thresh_ = rospy.get_param("~nav_yaw_vel_thresh", 1.0)
self.nav_pos_convergence_thresh_ = rospy.get_param(
"~nav_pos_convergence_thresh", 0.1)
self.nav_yaw_convergence_thresh_ = rospy.get_param(
"~nav_yaw_convergence_thresh", 0.05)
self.nav_vel_convergence_thresh_ = rospy.get_param(
"~nav_vel_convergence_thresh", 0.1)
self.takeoff_forward_offset_ = rospy.get_param(
"~takeoff_forward_offset", 4.0)
self.deep_return_dist_ = rospy.get_param("~deep_return_dist", -0.6)
self.turn_radius_offset_ = rospy.get_param("~turn_radius_offset", -1.0)
#circle motion
self.circle_radius_ = rospy.get_param("~circle_radius", 1.0)
self.circle_y_vel_ = rospy.get_param("~circle_y_vel", 0.5)
#obstacle avoidance
self.cluster_num_min_ = rospy.get_param("~cluster_num_min", 10)
self.safe_zone_radius_ = rospy.get_param("~safe_zone", 1.0)
self.drone_obstacle_ignore_maximum_radius_ = rospy.get_param(
"~drone_obstacle_ignore_maximum_radius", 0.90)
self.drone_safety_minimum_radius_ = rospy.get_param(
"~drone_safety_minimum_radius", 0.85)
self.avoid_vel_ = rospy.get_param("~avoid_vel", 0.5)
#flag
self.task_kind_ = rospy.get_param("~task_kind",
1) #1 yosen 2 honsen 3 kesshou
self.do_avoidance_ = rospy.get_param("~do_avoidance", False)
self.turn_before_return_ = rospy.get_param("~turn_before_return", True)
self.visualization_ = rospy.get_param("~visualization", True)
## task3 different
self.circle_motion_times_ = 1
self.circle_motion_height_step_ = 0
if self.task_kind_ == 2:
self.circle_motion_times_ = rospy.get_param(
"~circle_motion_times", 1)
self.circle_motion_height_step_ = rospy.get_param(
"~circle_motion_height_step_", 0.5)
self.target_num_ = 1
if self.task_kind_ == 3:
self.target_num_ = rospy.get_param("~target_num", 2)
self.turn_before_return_ = True
self.do_avoidance_ = True
self.vel_pub_ = rospy.Publisher(self.vel_pub_topic_name_,
Twist,
queue_size=10)
self.state_machine_pub_ = rospy.Publisher(
self.state_machine_pub_topic_name_, String, queue_size=10)
self.target_pos_pub_ = rospy.Publisher(self.target_pos_pub_topic_name_,
Odometry,
queue_size=10)
self.tracking_control_pub_ = rospy.Publisher(
self.tracking_control_pub_topic_name_, Bool, queue_size=1)
self.tree_detection_start_pub_ = rospy.Publisher(
self.tree_detection_start_pub_topic_name_, Bool, queue_size=10)
self.state_visualization_pub_ = rospy.Publisher(
self.state_visualization_pub_topic_name_,
OverlayText,
queue_size=10)
self.task_start_sub_ = rospy.Subscriber(
self.task_start_sub_topic_name_, Empty, self.taskStartCallback)
self.odom_sub_ = rospy.Subscriber(self.uav_odom_sub_topic_name_,
Odometry, self.odomCallback)
self.tree_location_sub_ = rospy.Subscriber(
self.tree_location_sub_topic_name_, PointStamped,
self.treeLocationCallback)
self.tree_cluster_sub_ = rospy.Subscriber(
self.tree_cluster_sub_topic_name_, LaserScan,
self.treeClusterCallback)
self.control_timer_ = rospy.Timer(
rospy.Duration(1.0 / self.control_rate_), self.controlCallback)
class ForestMotion:
def init(self):
if sys.argv[1] == 'True':
self.use_dji_ = True
else:
self.use_dji_ = False
if self.use_dji_ == True:
self.drone_ = DJIDrone()
self.drone_.request_sdk_permission_control()
rospy.loginfo("Use DJI")
else:
rospy.init_node('circle_motion', anonymous=True)
self.odom_ = Odometry()
self.GLOBAL_FRAME_ = 0
self.LOCAL_FRAME_ = 1
self.target_xy_pos_ = np.zeros(2)
self.target_z_pos_ = 0.0
self.target_yaw_ = 0.0
self.target_frame_ = self.GLOBAL_FRAME_
self.initial_xy_global_pos_ = np.zeros(2)
self.initial_z_pos_ = 0.0
self.initial_yaw_ = 0.0
#state machine
self.INITIAL_STATE_ = 0
self.TAKEOFF_STATE_ = 1
self.TREE_DETECTION_START_STATE_ = 2
self.APPROACHING_TO_TREE_STATE_ = 3
self.START_CIRCLE_MOTION_STATE_ = 4
self.FINISH_CIRCLE_MOTION_STATE_ = 5
self.TURN_STATE_ = 6
self.RETURN_HOME_STATE_ = 7
self.FINISH_STATE_ = 8
self.state_machine_ = self.INITIAL_STATE_
self.state_name_ = [
"initial", "takeoff", "tree detection start",
"approaching to tree", "circle motion", "finish circle motion",
"turn", "return home", "finish"
]
self.circle_motion_count_ = 0
self.target_count_ = 0
self.odom_update_flag_ = False
self.uav_xy_global_pos_ = np.zeros(2)
self.uav_z_pos_ = 0.0
self.uav_yaw_ = 0.0
self.uav_yaw_old_ = 0.0
self.uav_yaw_overflow_ = 0
self.uav_accumulated_yaw_ = 0.0
self.tree_cluster_ = LaserScan()
self.tree_xy_local_pos_ = np.zeros(2)
self.tree_pos_update_flag_ = False
self.task_start_ = False
self.task_start_time_ = rospy.Time.now()
self.task_elapsed_time_ = rospy.Time(0)
self.turn_uav_xy_global_pos_ = np.zeros(2)
self.vel_pub_topic_name_ = rospy.get_param("~vel_pub_topic_name",
"cmd_vel")
self.state_machine_pub_topic_name_ = rospy.get_param(
"~state_machine_pub_topic_name", "state_machine")
self.target_pos_pub_topic_name_ = rospy.get_param(
"~target_pos_pub_topic_name", "uav_target_pos")
self.tracking_control_pub_topic_name_ = rospy.get_param(
"~tracking_control_pub_topic_name", "/tracking_control")
self.state_visualization_pub_topic_name_ = rospy.get_param(
"state_visualization_pub_topic_name", "overlay_text")
self.task_start_sub_topic_name_ = rospy.get_param(
"~task_start_sub_topic_name", "task_start")
self.uav_odom_sub_topic_name_ = rospy.get_param(
"~uav_odom_sub_topic_name", "ground_truth/state")
self.tree_location_sub_topic_name_ = rospy.get_param(
"~tree_location_sub_topic_name", "tree_location")
self.tree_detection_start_pub_topic_name_ = rospy.get_param(
"~tree_detection_start_pub_topic_name", "detection_start")
self.tree_cluster_sub_topic_name_ = rospy.get_param(
"~tree_cluster_sub_topic_name", "scan_clustered")
self.update_target_tree_service_name_ = rospy.get_param(
"~update_target_tree_service_name", "/update_target_tree")
self.global_state_name_sub_topic_name_ = rospy.get_param(
"~global_state_name_sub_topic_name", "state_machine")
self.set_first_tree_service_name_ = rospy.get_param(
"~set_first_tree_service_name_", "/set_first_tree")
self.control_rate_ = rospy.get_param("~control_rate", 20)
#navigation
self.takeoff_height_ = rospy.get_param("~takeoff_height", 1.5)
self.return_height_ = rospy.get_param("~return_height", 1.5)
self.nav_xy_pos_pgain_ = rospy.get_param("~nav_xy_pos_pgain", 1.0)
self.nav_z_pos_pgain_ = rospy.get_param("~nav_z_pos_pgain", 1.0)
self.nav_yaw_pgain_ = rospy.get_param("~nav_yaw_pgain", 1.0)
self.nav_xy_vel_thresh_ = rospy.get_param("~nav_xy_vel_thresh", 2.0)
self.nav_z_vel_thresh_ = rospy.get_param("~nav_z_vel_thresh", 2.0)
self.nav_yaw_vel_thresh_ = rospy.get_param("~nav_yaw_vel_thresh", 1.0)
self.nav_pos_convergence_thresh_ = rospy.get_param(
"~nav_pos_convergence_thresh", 0.1)
self.nav_yaw_convergence_thresh_ = rospy.get_param(
"~nav_yaw_convergence_thresh", 0.05)
self.nav_vel_convergence_thresh_ = rospy.get_param(
"~nav_vel_convergence_thresh", 0.1)
self.takeoff_forward_offset_ = rospy.get_param(
"~takeoff_forward_offset", 4.0)
self.deep_return_dist_ = rospy.get_param("~deep_return_dist", -0.6)
self.turn_radius_offset_ = rospy.get_param("~turn_radius_offset", -1.0)
#circle motion
self.circle_radius_ = rospy.get_param("~circle_radius", 1.0)
self.circle_y_vel_ = rospy.get_param("~circle_y_vel", 0.5)
#obstacle avoidance
self.cluster_num_min_ = rospy.get_param("~cluster_num_min", 10)
self.safe_zone_radius_ = rospy.get_param("~safe_zone", 1.0)
self.drone_obstacle_ignore_maximum_radius_ = rospy.get_param(
"~drone_obstacle_ignore_maximum_radius", 0.90)
self.drone_safety_minimum_radius_ = rospy.get_param(
"~drone_safety_minimum_radius", 0.85)
self.avoid_vel_ = rospy.get_param("~avoid_vel", 0.5)
#flag
self.task_kind_ = rospy.get_param("~task_kind",
1) #1 yosen 2 honsen 3 kesshou
self.do_avoidance_ = rospy.get_param("~do_avoidance", False)
self.turn_before_return_ = rospy.get_param("~turn_before_return", True)
self.visualization_ = rospy.get_param("~visualization", True)
## task3 different
self.circle_motion_times_ = 1
self.circle_motion_height_step_ = 0
if self.task_kind_ == 2:
self.circle_motion_times_ = rospy.get_param(
"~circle_motion_times", 1)
self.circle_motion_height_step_ = rospy.get_param(
"~circle_motion_height_step_", 0.5)
self.target_num_ = 1
if self.task_kind_ == 3:
self.target_num_ = rospy.get_param("~target_num", 2)
self.turn_before_return_ = True
self.do_avoidance_ = True
self.vel_pub_ = rospy.Publisher(self.vel_pub_topic_name_,
Twist,
queue_size=10)
self.state_machine_pub_ = rospy.Publisher(
self.state_machine_pub_topic_name_, String, queue_size=10)
self.target_pos_pub_ = rospy.Publisher(self.target_pos_pub_topic_name_,
Odometry,
queue_size=10)
self.tracking_control_pub_ = rospy.Publisher(
self.tracking_control_pub_topic_name_, Bool, queue_size=1)
self.tree_detection_start_pub_ = rospy.Publisher(
self.tree_detection_start_pub_topic_name_, Bool, queue_size=10)
self.state_visualization_pub_ = rospy.Publisher(
self.state_visualization_pub_topic_name_,
OverlayText,
queue_size=10)
self.task_start_sub_ = rospy.Subscriber(
self.task_start_sub_topic_name_, Empty, self.taskStartCallback)
self.odom_sub_ = rospy.Subscriber(self.uav_odom_sub_topic_name_,
Odometry, self.odomCallback)
self.tree_location_sub_ = rospy.Subscriber(
self.tree_location_sub_topic_name_, PointStamped,
self.treeLocationCallback)
self.tree_cluster_sub_ = rospy.Subscriber(
self.tree_cluster_sub_topic_name_, LaserScan,
self.treeClusterCallback)
self.control_timer_ = rospy.Timer(
rospy.Duration(1.0 / self.control_rate_), self.controlCallback)
def taskStartCallback(self, msg):
rospy.loginfo("Task Start")
self.task_start_time_ = rospy.Time.now()
self.task_start_ = True
self.task_start_sub_.unregister()
def odomCallback(self, msg):
self.odom_ = msg
self.uav_xy_global_pos_ = np.array(
[msg.pose.pose.position.x, msg.pose.pose.position.y])
self.uav_z_pos_ = msg.pose.pose.position.z
quaternion = np.array([
msg.pose.pose.orientation.x, msg.pose.pose.orientation.y,
msg.pose.pose.orientation.z, msg.pose.pose.orientation.w
])
self.uav_yaw_ = tf.transformations.euler_from_quaternion(quaternion)[2]
if self.odom_update_flag_ == True:
if self.uav_yaw_ - self.uav_yaw_old_ > 5.0:
self.uav_yaw_overflow_ -= 1
elif self.uav_yaw_old_ - self.uav_yaw_old_ < -5.0:
self.uav_yaw_overflow_ += 1
self.uav_accumulated_yaw_ = self.uav_yaw_ + 2 * math.pi * self.uav_yaw_overflow_
self.uav_yaw_old_ = self.uav_yaw_
self.odom_update_flag_ = True
def treeDetectionStart(self):
msg = Bool()
msg.data = True
self.tree_detection_start_pub_.publish(msg)
def treeClusterCallback(self, msg):
self.tree_cluster_ = msg
def treeLocationCallback(self, msg):
self.tree_pos_update_flag_ = True
self.tree_xy_local_pos_ = np.array([msg.point.x, msg.point.y])
def isConvergent(self, frame, target_xy_pos, target_z_pos, target_yaw):
if frame == self.GLOBAL_FRAME_:
#delta_pos = np.array([target_xy_pos[0] - self.uav_xy_global_pos_[0], target_xy_pos[1] - self.uav_xy_global_pos_[1], target_z_pos - self.uav_z_pos_])
delta_pos = np.array([
target_xy_pos[0] - self.uav_xy_global_pos_[0],
target_xy_pos[1] - self.uav_xy_global_pos_[1], 0
])
elif frame == self.LOCAL_FRAME_:
#delta_pos = np.array([target_xy_pos[0], target_xy_pos[1], target_z_pos - self.uav_z_pos_])
delta_pos = np.array([target_xy_pos[0], target_xy_pos[1], 0])
else:
return
delta_yaw = target_yaw - self.uav_yaw_
if delta_yaw > math.pi:
delta_yaw -= math.pi * 2
elif delta_yaw < -math.pi:
delta_yaw += math.pi * 2
current_vel = np.array([
self.odom_.twist.twist.linear.x, self.odom_.twist.twist.linear.y,
self.odom_.twist.twist.linear.z
])
if np.linalg.norm(
delta_pos) < self.nav_pos_convergence_thresh_ and abs(
delta_yaw
) < self.nav_yaw_convergence_thresh_ and np.linalg.norm(
current_vel) < self.nav_vel_convergence_thresh_:
return True
else:
return False
def saturateVelocity(self, xy_vel, z_vel, yaw_vel):
ret = [xy_vel, z_vel, yaw_vel]
if np.linalg.norm(xy_vel) > self.nav_xy_vel_thresh_:
ret[0] = xy_vel * (self.nav_xy_vel_thresh_ /
np.linalg.norm(xy_vel))
if abs(z_vel) > self.nav_z_vel_thresh_:
ret[1] = z_vel * self.nav_z_vel_thresh_ / abs(z_vel)
if abs(yaw_vel) > self.nav_yaw_vel_thresh_:
ret[2] = yaw_vel * self.nav_yaw_vel_thresh_ / abs(yaw_vel)
return ret
#go pos function: z and yaw are always in global frame
def goPos(self, frame, target_xy, target_z, target_yaw):
if frame == self.GLOBAL_FRAME_:
rot_mat = np.array(
[[math.cos(self.uav_yaw_),
math.sin(self.uav_yaw_)],
[-math.sin(self.uav_yaw_),
math.cos(self.uav_yaw_)]])
delta_xy = np.dot(rot_mat, target_xy - self.uav_xy_global_pos_)
elif frame == self.LOCAL_FRAME_:
delta_xy = target_xy
else:
return
delta_z = target_z - self.uav_z_pos_
delta_yaw = target_yaw - self.uav_yaw_
if delta_yaw > math.pi:
delta_yaw -= math.pi * 2
elif delta_yaw < -math.pi:
delta_yaw += math.pi * 2
nav_xy_vel = delta_xy * self.nav_xy_pos_pgain_
nav_z_vel = delta_z * self.nav_z_pos_pgain_
nav_yaw_vel = delta_yaw * self.nav_yaw_pgain_
nav_xy_vel, nav_z_vel, nav_yaw_vel = self.saturateVelocity(
nav_xy_vel, nav_z_vel, nav_yaw_vel)
vel_msg = Twist()
vel_msg.linear.x = nav_xy_vel[0]
vel_msg.linear.y = nav_xy_vel[1]
vel_msg.linear.z = nav_z_vel
vel_msg.angular.z = nav_yaw_vel
self.target_xy_pos_ = target_xy
self.target_z_pos_ = target_z
self.target_yaw_ = target_yaw
self.target_frame_ = frame
return vel_msg
def goCircle(self, circle_center_local_xy, target_z, target_y_vel, radius):
nav_xy_vel = np.zeros(2)
nav_xy_vel[0] = -(radius -
circle_center_local_xy[0]) * self.nav_xy_pos_pgain_
nav_xy_vel[1] = target_y_vel
nav_z_vel = target_z - self.uav_z_pos_
nav_yaw_vel = math.atan2(
circle_center_local_xy[1], circle_center_local_xy[0]
) * self.nav_yaw_pgain_ - target_y_vel / radius #feedback+feedforward
nav_xy_vel, nav_z_vel, nav_yaw_vel = self.saturateVelocity(
nav_xy_vel, nav_z_vel, nav_yaw_vel)
vel_msg = Twist()
vel_msg.linear.x = nav_xy_vel[0]
vel_msg.linear.y = nav_xy_vel[1]
vel_msg.linear.z = nav_z_vel
vel_msg.angular.z = nav_yaw_vel
return vel_msg
def obstacleDetection(self):
if not self.do_avoidance_:
return [False]
if self.state_machine_ != self.TAKEOFF_STATE_ and self.state_machine_ != self.APPROACHING_TO_TREE_STATE_ and self.state_machine_ != self.RETURN_HOME_STATE_:
return [False]
## If closed to the target tree within a certain distance, that is safe zone,
## we do not need to the obstacle avoidance.
dist = np.linalg.norm(self.target_xy_pos_)
if self.target_frame_ == self.GLOBAL_FRAME_:
dist = np.linalg.norm(self.target_xy_pos_ -
self.uav_xy_global_pos_)
if dist < self.safe_zone_radius_:
#rospy.loginfo("safe zone, distance :%f", dist)
return [False]
nearest_obstacle_distance_to_head_direction = 0.0
nearest_obstacle_distance_in_head_direction = 10000.0
nearest_obstacle_id = -1
## TODO: add changable obstacle_ignore radius, since 1m is too near for normal obstacle
for i in range(0, len(self.tree_cluster_.ranges)):
if self.tree_cluster_.ranges[i] > 0:
# check whether the obstacle is big enough
big_cluster = True
for j in range(-self.cluster_num_min_ / 2,
self.cluster_num_min_ / 2):
if math.isnan(self.tree_cluster_.ranges[i + j]):
big_cluster = False
break
if not big_cluster:
continue
if abs(self.tree_cluster_.ranges[i]
) < self.drone_obstacle_ignore_maximum_radius_:
obstacle_angle = i * self.tree_cluster_.angle_increment + self.tree_cluster_.angle_min
obstacle_distance_to_head_direction = abs(
self.tree_cluster_.ranges[i] *
math.sin(obstacle_angle))
if obstacle_distance_to_head_direction < self.drone_safety_minimum_radius_:
## the closest obstacle in head direction influence in earlist time
obstacle_distance_in_head_direction = abs(
self.tree_cluster_.ranges[i]) * math.cos(
obstacle_angle)
if obstacle_distance_in_head_direction < nearest_obstacle_distance_in_head_direction:
nearest_obstacle_distance_in_head_direction = obstacle_distance_in_head_direction
nearest_obstacle_distance_to_head_direction = obstacle_distance_to_head_direction
nearest_obstacle_id = i
if nearest_obstacle_id >= 0:
nearest_obstacle_angle = nearest_obstacle_id * self.tree_cluster_.angle_increment + self.tree_cluster_.angle_min
# rospy.logwarn("Meet obstacle: [%.3f, %.3f] -> %.3f[m], %.3f[deg] in phase: %s", nearest_obstacle_distance_in_head_direction, nearest_obstacle_distance_to_head_direction, self.tree_cluster_.ranges[nearest_obstacle_id], nearest_obstacle_angle / math.pi * 180.0, self.state_name_[self.state_machine_])
return [True, nearest_obstacle_angle]
else:
return [False]
def controlCallback(self, event):
if (not self.odom_update_flag_) or (not self.task_start_):
return
#navigation
vel_msg = Twist()
if self.state_machine_ == self.INITIAL_STATE_:
vel_msg.linear.x = vel_msg.linear.y = vel_msg.linear.z = vel_msg.angular.z = 0.0
if self.state_machine_ == self.TAKEOFF_STATE_:
rot_mat = np.array(
[[math.cos(self.initial_yaw_), -math.sin(self.initial_yaw_)],
[math.sin(self.initial_yaw_),
math.cos(self.initial_yaw_)]])
takeoff_xy_global_pos_ = self.initial_xy_global_pos_ + np.dot(
rot_mat, np.array([self.takeoff_forward_offset_, 0]))
vel_msg = self.goPos(self.GLOBAL_FRAME_, takeoff_xy_global_pos_,
self.takeoff_height_, self.initial_yaw_)
if self.state_machine_ == self.TREE_DETECTION_START_STATE_:
vel_msg = self.goPos(self.GLOBAL_FRAME_, self.target_xy_pos_,
self.target_z_pos_, self.target_yaw_) #hover
if self.state_machine_ == self.APPROACHING_TO_TREE_STATE_:
tree_direction = math.atan2(self.tree_xy_local_pos_[1],
self.tree_xy_local_pos_[0])
vel_msg = self.goPos(
self.LOCAL_FRAME_,
np.array([
self.tree_xy_local_pos_[0] -
self.circle_radius_ * math.cos(tree_direction),
self.tree_xy_local_pos_[1] -
self.circle_radius_ * math.sin(tree_direction)
]), self.takeoff_height_, self.uav_yaw_ + tree_direction)
if self.state_machine_ == self.START_CIRCLE_MOTION_STATE_:
vel_msg = self.goCircle(
self.tree_xy_local_pos_, self.takeoff_height_ +
self.circle_motion_count_ * self.circle_motion_height_step_,
self.circle_y_vel_, self.circle_radius_)
if self.state_machine_ == self.FINISH_CIRCLE_MOTION_STATE_:
tree_direction = math.atan2(self.tree_xy_local_pos_[1],
self.tree_xy_local_pos_[0])
vel_msg = self.goPos(
self.LOCAL_FRAME_,
np.array([
self.tree_xy_local_pos_[0] - self.circle_radius_,
self.tree_xy_local_pos_[1]
]), self.takeoff_height_, self.circle_initial_yaw_)
if self.state_machine_ == self.TURN_STATE_:
vel_msg = self.goPos(self.GLOBAL_FRAME_,
self.turn_uav_xy_global_pos_,
self.takeoff_height_,
self.initial_yaw_ + math.pi)
if self.state_machine_ == self.RETURN_HOME_STATE_:
if self.turn_before_return_ == False:
#use local frame
vel_msg = self.goPos(
self.LOCAL_FRAME_, self.tree_xy_local_pos_ -
self.initial_target_tree_xy_local_pos_ -
np.array([self.takeoff_forward_offset_, 0]),
self.return_height_, self.initial_yaw_)
else:
#use global fram
vel_msg = self.goPos(
self.GLOBAL_FRAME_, self.final_xy_global_pos_ +
self.final_target_tree_xy_global_pos_ -
self.initial_target_tree_xy_global_pos_,
self.return_height_, self.initial_yaw_ + math.pi)
# obstacle avoidance
obstacle = self.obstacleDetection()
if obstacle[0]:
# print "avoidance activate"
## if nearest obstacle is on drone right side, drone moves left
if obstacle[1] < 0.0:
vel_msg = self.goPos(self.LOCAL_FRAME_,
np.array([0.0, self.avoid_vel_]),
self.target_z_pos_, self.target_yaw_)
## if nearest obstacle is on drone left side, drone moves right
else:
vel_msg = self.goPos(self.LOCAL_FRAME_,
np.array([0.0, -self.avoid_vel_]),
self.target_z_pos_, self.target_yaw_)
#end navigation
#state machine
if self.state_machine_ == self.INITIAL_STATE_:
self.initial_xy_global_pos_ = np.array(self.uav_xy_global_pos_)
rot_mat = np.array(
[[math.cos(self.uav_yaw_), -math.sin(self.uav_yaw_)],
[math.sin(self.uav_yaw_),
math.cos(self.uav_yaw_)]])
self.final_xy_global_pos_ = np.dot(
rot_mat, np.array([self.deep_return_dist_, 0
])) + self.initial_xy_global_pos_
self.initial_yaw_ = self.uav_yaw_
if self.use_dji_ == True:
self.drone_.takeoff()
self.state_machine_ = self.TAKEOFF_STATE_
rospy.loginfo("take off")
elif self.state_machine_ == self.TAKEOFF_STATE_:
if self.isConvergent(self.target_frame_, self.target_xy_pos_,
self.target_z_pos_, self.target_yaw_):
self.treeDetectionStart()
self.state_machine_ = self.TREE_DETECTION_START_STATE_
elif self.state_machine_ == self.TREE_DETECTION_START_STATE_:
if self.tree_pos_update_flag_ == True:
self.state_machine_ = self.APPROACHING_TO_TREE_STATE_
rot_mat = np.array(
[[math.cos(self.uav_yaw_), -math.sin(self.uav_yaw_)],
[math.sin(self.uav_yaw_),
math.cos(self.uav_yaw_)]])
self.initial_target_tree_xy_global_pos_ = np.dot(
rot_mat, self.tree_xy_local_pos_) + self.uav_xy_global_pos_
self.initial_target_tree_xy_local_pos_ = self.tree_xy_local_pos_
elif self.state_machine_ == self.APPROACHING_TO_TREE_STATE_:
if self.isConvergent(self.target_frame_, self.target_xy_pos_,
self.target_z_pos_, self.target_yaw_):
self.circle_initial_accumulated_yaw_ = self.uav_accumulated_yaw_
self.circle_initial_yaw_ = self.uav_yaw_
self.circle_initial_xy_ = np.array(self.uav_xy_global_pos_)
if self.task_kind_ == 1:
self.state_machine_ = self.FINISH_CIRCLE_MOTION_STATE_
elif self.task_kind_ > 1:
if self.task_kind_ == 3 and self.target_count_ == self.target_num_:
self.state_machine_ = self.FINISH_CIRCLE_MOTION_STATE_
else:
self.state_machine_ = self.START_CIRCLE_MOTION_STATE_
elif self.state_machine_ == self.START_CIRCLE_MOTION_STATE_:
if abs(self.circle_initial_accumulated_yaw_ -
self.uav_accumulated_yaw_) > 2 * math.pi:
self.circle_initial_accumulated_yaw_ = self.uav_accumulated_yaw_
if self.task_kind_ == 2:
self.circle_motion_count_ += 1
if self.circle_motion_count_ == self.circle_motion_times_:
self.state_machine_ = self.FINISH_CIRCLE_MOTION_STATE_
else:
self.state_machine_ = self.FINISH_CIRCLE_MOTION_STATE_
elif self.state_machine_ == self.FINISH_CIRCLE_MOTION_STATE_:
if self.isConvergent(self.target_frame_, self.target_xy_pos_,
self.target_z_pos_, self.target_yaw_):
# task3 special process
self.target_count_ += 1
if self.task_kind_ == 3:
if self.target_count_ < self.target_num_:
rospy.wait_for_service(
self.update_target_tree_service_name_)
try:
update_target_tree = rospy.ServiceProxy(
self.update_target_tree_service_name_, Trigger)
res = update_target_tree()
if res.success:
self.state_machine_ = self.TREE_DETECTION_START_STATE_
self.tree_pos_update_flag_ = False
time.sleep(0.5)
return
else:
self.target_count_ = self.target_num_
rospy.logerr("can not get next target tree")
except rospy.ServiceException, e:
print "Service call failed: %s" % e
if self.target_count_ == self.target_num_:
rospy.wait_for_service(
self.set_first_tree_service_name_)
try:
set_first_tree = rospy.ServiceProxy(
self.set_first_tree_service_name_, Trigger)
res = set_first_tree()
if res.success:
self.state_machine_ = self.TREE_DETECTION_START_STATE_
self.tree_pos_update_flag_ = False
time.sleep(0.5)
return
except rospy.ServiceException, e:
print "Service call failed: %s" % e
if self.turn_before_return_ == True:
rot_mat = np.array(
[[math.cos(self.uav_yaw_), -math.sin(self.uav_yaw_)],
[math.sin(self.uav_yaw_),
math.cos(self.uav_yaw_)]])
self.final_target_tree_xy_global_pos_ = np.dot(
rot_mat,
self.tree_xy_local_pos_) + self.uav_xy_global_pos_
rot_mat = np.array([[
math.cos(self.initial_yaw_),
-math.sin(self.initial_yaw_)
],
[
math.sin(self.initial_yaw_),
math.cos(self.initial_yaw_)
]])
if self.task_kind_ != 3:
self.turn_uav_xy_global_pos_ = np.dot(
rot_mat, np.array([self.turn_radius_offset_, 0
])) + self.uav_xy_global_pos_
else:
self.turn_uav_xy_global_pos_ = self.uav_xy_global_pos_
#self.turn_uav_yaw_ = self.uav_yaw_
self.state_machine_ = self.TURN_STATE_
# stop tree tracking if necessary
stop_msg = Bool()
self.tracking_control_pub_.publish(stop_msg)
else:
self.state_machine_ = self.RETURN_HOME_STATE_