Skip to content

Commit

Permalink
Add motion planning options inside circle_motion.py.
Browse files Browse the repository at this point in the history 
Add environment for collision test.
  • Loading branch information
@tongtybj
KahnShi authored and tongtybj committed Apr 28, 2017
1 parent 18c5572 commit 06770a9
Show file tree
Hide file tree
Showing 5 changed files with 220 additions and 365 deletions.
22 changes: 8 additions & 14 deletions jsk_uav_forest_motion/launch/motion.launch
View file
Original file line number Diff line number Diff line change
Expand Up @@ -15,22 +15,16 @@
</include>

##### Motion Node #####
<group unless="$(arg motion_planning)">
<node pkg="jsk_uav_forest_motion" type="circle_motion.py" name="circle_motion" output="screen" args="$(arg use_dji)">
<param name="task_kind" value="$(arg task_kind)" />
<param name="circle_motion_times" value="$(arg circle_motion_times)" />
<param name="uav_odom_sub_topic_name" value="$(arg uav_odom_topic_name)" if="$(arg use_dji)"/>
<param name="turn_before_return" value="$(arg turn_before_return)" />
<rosparam file="$(find jsk_uav_forest_motion)/config/circle_motion/circle_motion.yaml" command="load"/>
</node>
</group>
<node pkg="jsk_uav_forest_motion" type="circle_motion.py" name="circle_motion" output="screen" args="$(arg use_dji)">
<param name="task_kind" value="$(arg task_kind)" />
<param name="motion_planning" value="$(arg motion_planning)" />
<param name="circle_motion_times" value="$(arg circle_motion_times)" />
<param name="uav_odom_sub_topic_name" value="$(arg uav_odom_topic_name)" if="$(arg use_dji)"/>
<param name="turn_before_return" value="$(arg turn_before_return)" />
<rosparam file="$(find jsk_uav_forest_motion)/config/circle_motion/circle_motion.yaml" command="load"/>
</node>

<group if="$(arg motion_planning)">
<node pkg="jsk_uav_forest_motion" type="circle_motion_server.py" name="circle_motion_server" output="screen" args="$(arg use_dji)">
<param name="task_kind" value="$(arg task_kind)" />
<param name="uav_odom_sub_topic_name" value="$(arg uav_odom_topic_name)" if="$(arg use_dji)"/>
<rosparam file="$(find jsk_uav_forest_motion)/config/circle_motion/circle_motion.yaml" command="load"/>
</node>
<node pkg="jsk_uav_forest_motion" type="motion_plannar_client.py" name="motion_plannar" output="screen" args="$(arg use_dji)">
<param name="task_kind" value="$(arg task_kind)" />
<param name="uav_odom_sub_topic_name" value="$(arg uav_odom_topic_name)" if="$(arg use_dji)"/>
Expand Down
51 changes: 39 additions & 12 deletions jsk_uav_forest_motion/script/circle_motion.py
View file
Original file line number Diff line number Diff line change
Expand Up @@ -75,6 +75,7 @@ def init(self):
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.plannar_mode_ = rospy.get_param("~motion_planning", False)
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)
Expand Down Expand Up @@ -103,6 +104,9 @@ def init(self):
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)

if self.plannar_mode_:
rospy.loginfo("Using motion planning.")

def odomCallback(self, msg):
self.odom_ = msg
self.uav_xy_pos_ = np.array([msg.pose.pose.position.x, msg.pose.pose.position.y])
Expand Down Expand Up @@ -209,6 +213,12 @@ def goPos(self, frame, target_xy, target_z, target_yaw):

return vel_msg

def setGoal(self, frame, target_xy, target_z, target_yaw):
self.target_xy_pos_ = target_xy
self.target_z_pos_ = target_z
self.target_yaw_ = target_yaw
self.target_frame_ = frame

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_
Expand Down Expand Up @@ -247,11 +257,19 @@ def controlCallback(self, event):
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 not self.plannar_mode_:
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)
else:
self.setGoal(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_:
Expand All @@ -266,12 +284,19 @@ def controlCallback(self, event):
if self.state_machine_ == self.TURN_STATE_:
vel_msg = self.goPos(self.GLOBAL_FRAME_, self.turn_uav_xy_pos_, self.takeoff_height_, self.turn_uav_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_pos_ - self.initial_target_tree_xy_local_pos_, self.takeoff_height_, self.initial_yaw_)
if not self.plannar_mode_:
if self.turn_before_return_ == False:
#use local frame
vel_msg = self.goPos(self.LOCAL_FRAME_, self.tree_xy_pos_ - self.initial_target_tree_xy_local_pos_, self.takeoff_height_, self.initial_yaw_)
else:
#use global frame
vel_msg = self.goPos(self.GLOBAL_FRAME_, self.initial_xy_pos_ + self.final_target_tree_xy_global_pos_ - self.initial_target_tree_xy_global_pos_, self.takeoff_height_, self.turn_uav_yaw_ + math.pi)
else:
#use global frame
vel_msg = self.goPos(self.GLOBAL_FRAME_, self.initial_xy_pos_ + self.final_target_tree_xy_global_pos_ - self.initial_target_tree_xy_global_pos_, self.takeoff_height_, self.turn_uav_yaw_ + math.pi)
if self.turn_before_return_ == False:
#use local frame
self.setGoal(self.LOCAL_FRAME_, self.tree_xy_pos_ - self.initial_target_tree_xy_pos_, self.takeoff_height_, self.initial_yaw_)
else:
self.setGoal(self.GLOVAL_FRAME_, self.initial_xy_pos_ + self.final_target_tree_xy_global_pos_ - self.initial_target_tree_xy_global_pos_, self.takeoff_height_, self.turn_uav_yaw_ + math.pi)
#end navigation

#state machine
Expand Down Expand Up @@ -345,9 +370,11 @@ def controlCallback(self, event):
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]
## consider obstacle avoidance in APPROACHING_TO_TREE_STATE and RETURN_HOME_STATE, velocity will be controled in motion_plannar_client
if not(self.plannar_mode_ and (self.state_machine_ == self.APPROACHING_TO_TREE_STATE_ or self.state_machine_ == self.RETURN_HOME_STATE_)):
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]

if __name__ == '__main__':
try:
Expand Down
Loading

0 comments on commit 06770a9

Please sign in to comment.