# OMPL Plannification: /home/blue/GitLab Repo/OMPL_Planning/Source Files/mapping_planning_tutorial/control_turtlebot/src/controller_quadrotor.py Source File

controller_quadrotor.py
#!/usr/bin/env python
# ROS imports
import roslib; roslib.load_manifest('control_turtlebot')
import rospy
import tf
import math
#ROS messages
from geometry_msgs.msg import Twist
from nav_msgs.msg import Odometry
from control_turtlebot.srv import GotoWaypoint, GotoWaypointResponse, FindPathToGoal, FindPathToGoalResponse, FindPathToGoalRequest
#Numpy
import numpy as np
class Controller(object):
def __init__(self):
self.odometry_sub_ = rospy.Subscriber("/ground_truth/state", Odometry, self.odomCallback, queue_size = 1)
self.control_input_pub_ = rospy.Publisher("/cmd_vel", Twist, queue_size = 1)
self.serv_ = rospy.Service('/controller_turtlebot/goto',
GotoWaypoint,
self.calculateControlInput)
self.current_position_ = np.zeros(3)
self.current_orientation_ = 0.0
self.desired_position_ = np.zeros(3)
self.desired_orientation_ = 0.0
rospy.wait_for_service('/controller_turtlebot/find_path_to_goal')
try:
self.find_path_to_goal_serv_ = rospy.ServiceProxy('/controller_turtlebot/find_path_to_goal', FindPathToGoal)
except rospy.ServiceException, e:
print "Service call failed: %s"%e
return
def calculateControlInput(self, req):
planner_request = FindPathToGoalRequest()
planner_request.new_goal = True
planner_request.goal_state_x = req.goal_state_x
planner_request.goal_state_y = req.goal_state_y
planner_request.goal_state_z = req.goal_state_z
planner_response = self.find_path_to_goal_serv_(planner_request)
for pose in planner_response.poses:
print pose
control_input = Twist()
self.desired_position_[0] = pose.position.x
self.desired_position_[1] = pose.position.y
self.desired_position_[2] = pose.position.z
loop_rate = rospy.Rate(100) # 10Hz
orientation_approach = False
while not rospy.is_shutdown():
inc_x = self.desired_position_[0] - self.current_position_[0]
inc_y = self.desired_position_[1] - self.current_position_[1]
inc_z = self.desired_position_[2] - self.current_position_[2]
self.desired_orientation_ = wrapAngle(math.atan2(inc_y, inc_x))
yaw_error = wrapAngle(self.desired_orientation_ - self.current_orientation_)
distance_to_goal = math.sqrt(math.pow(inc_x, 2.0) + math.pow(inc_y, 2.0))
if abs(yaw_error) > 0.04 and not orientation_approach:
control_input.angular.x = 0.0
control_input.angular.y = 0.0
control_input.angular.z = yaw_error * 1.0
control_input.linear.x = 0.0
control_input.linear.y = 0.0
control_input.linear.z = 0.0
else:
control_input.angular.x = 0.0
control_input.angular.y = 0.0
control_input.angular.z = 0.0
orientation_approach = True
liner_speed = abs(distance_to_goal) * 0.5
altitude_speed = inc_z * 0.5
if liner_speed < 0.1:
control_input.linear.x = 0.1
elif liner_speed > 0.2:
control_input.linear.x = 0.2
else :
control_input.linear.x = liner_speed
if abs(altitude_speed) < 0.1:
if altitude_speed ==0 :
control_input.linear.z = 0
elif altitude_speed > 0 :
control_input.linear.z = 0.1
else:
control_input.linear.z = -0.1
elif abs(altitude_speed) > 0.2:
if altitude_speed > 0 :
control_input.linear.z = 0.2
else:
control_input.linear.z = - 0.2
else:
control_input.linear.z = altitude_speed
control_input.linear.y = 0.0
self.control_input_pub_.publish(control_input)
rospy.logdebug("%s: current position: [%f, %f]\n", rospy.get_name(), self.current_position_[0], self.current_position_[1])
rospy.logdebug("%s: desired position: [%f, %f]\n", rospy.get_name(), self.desired_position_[0], self.desired_position_[1])
rospy.logdebug("%s: yaw_error: %f\n", rospy.get_name(), yaw_error)
rospy.logdebug("%s: current orientation: %f\n", rospy.get_name(), self.current_orientation_)
rospy.logdebug("%s: desired orientation: %f\n", rospy.get_name(), self.desired_orientation_)
rospy.logdebug("%s: distance_to_goal: %f\n", rospy.get_name(), distance_to_goal)
rospy.logdebug("%s: control_input.linear.x %f\n", rospy.get_name(), control_input.linear.x)
rospy.logdebug("%s: control_input.angular.z %f\n", rospy.get_name(), control_input.angular.z)
if distance_to_goal <= 0.2:
break
loop_rate.sleep()
return GotoWaypointResponse()
def odomCallback(self, odometry_msg):
self.current_position_[0] = odometry_msg.pose.pose.position.x
self.current_position_[1] = odometry_msg.pose.pose.position.y
self.current_position_[2] = odometry_msg.pose.pose.position.z
(r, p, y) = tf.transformations.euler_from_quaternion([odometry_msg.pose.pose.orientation.x, odometry_msg.pose.pose.orientation.y, odometry_msg.pose.pose.orientation.z, odometry_msg.pose.pose.orientation.w])
self.current_orientation_ = wrapAngle(y)
return
def wrapAngle(angle):
"""wrapAngle
Calculates angles values between 0 and 2pi"""
return (angle + ( 2.0 * math.pi * math.floor( ( math.pi - angle ) / ( 2.0 * math.pi ) ) ) )
if __name__ == '__main__':
rospy.init_node('control_quadrotor', log_level=rospy.INFO)
rospy.loginfo("%s: starting quadrotor controller", rospy.get_name())
controller = Controller()
rospy.spin()
controller_quadrotor.Controller.calculateControlInput
def calculateControlInput(self, req)
Definition: controller_quadrotor.py:42
controller_quadrotor.Controller.desired_orientation_
desired_orientation_
Definition: controller_quadrotor.py:31
controller_quadrotor.Controller.control_input_pub_
control_input_pub_
Definition: controller_quadrotor.py:21
controller_quadrotor.Controller
Definition: controller_quadrotor.py:17
controller_quadrotor.Controller.serv_
serv_
Definition: controller_quadrotor.py:23
controller_quadrotor.Controller.odometry_sub_
odometry_sub_
Definition: controller_quadrotor.py:20
controller_quadrotor.Controller.current_position_
current_position_
Definition: controller_quadrotor.py:27
controller_quadrotor.Controller.odomCallback
def odomCallback(self, odometry_msg)
Definition: controller_quadrotor.py:130
controller_quadrotor.Controller.current_orientation_
current_orientation_
Definition: controller_quadrotor.py:28
controller_quadrotor.Controller.find_path_to_goal_serv_
find_path_to_goal_serv_
Definition: controller_quadrotor.py:36
controller_quadrotor.Controller.desired_position_
desired_position_
Definition: controller_quadrotor.py:30