#!/usr/bin/env python from geometry_msgs.msg import PoseStamped, TwistStamped, Point import rospy import math from std_msgs.msg import Float32, Bool, String from VelocityController import VelocityController import numpy as np class position_control(): def __init__(self): print 'Initialising position control' rospy.init_node('position_control', anonymous=True) rate = rospy.Rate(20) rospy.Subscriber('/position_control/set_mode', String, self.set_mode_callback) rospy.Subscriber('/position_control/set_position', PoseStamped, self.set_pose_callback) rospy.Subscriber('/position_control/set_velocity', PoseStamped, self.set_velocity_callback) rospy.Subscriber('/position_control/set_x_pid', Point, self.set_x_pid) rospy.Subscriber('/position_control/set_y_pid', Point, self.set_y_pid) rospy.Subscriber('/position_control/set_z_pid', Point, self.set_z_pid) # pos self._pose_pub = rospy.Publisher('mavros/setpoint_position/local', PoseStamped, queue_size=10) self._pose_msg = PoseStamped() self._vel_pose_msg = PoseStamped() self._pos_state = "posctr" # vel self._vel_pub = rospy.Publisher('mavros/setpoint_velocity/cmd_vel', TwistStamped, queue_size=10) self._vel_msg = TwistStamped() self._vel_state = "velctr" self.local_pose = PoseStamped() rospy.Subscriber('/mavros/local_position/pose', PoseStamped, self._local_pose_callback) self.local_velocity = TwistStamped() rospy.Subscriber('/mavros/local_position/velocity', TwistStamped, self._local_velocity_callback) self.dist = rospy.Publisher('/position_control/distance', Bool, queue_size=10) self.current_publisher = self._pose_pub self.current_message = self._pose_msg #self._pose_msg.pose.position.z = 3 self.set_pose(self._pose_msg) self.des_vel = TwistStamped() self.current_mode = String() self.current_mode.data = 'posctr' self.vController = VelocityController() self.vController.set_x_pid(2.8, 0.913921, 0.0, 0.5) self.vController.set_y_pid(2.8, 0.913921, 0.0, 0.5)#2.1, 0.713921, 0.350178 self.vController.set_z_pid(1.3, 2.4893, 0.102084, 0.5) print 'Init done' while not rospy.is_shutdown(): if self.current_mode.data == 'posctr': self._pose_pub.publish(self._pose_msg) elif self.current_mode.data == 'velctr': self.vController.setTarget(self._vel_pose_msg.pose) self.des_vel = self.vController.update(self.local_pose) self._vel_pub.publish(self.des_vel) else: print "No such position mode" self.check_distance() #self.current_publisher.publish(self.current_message) #self.check_distance() rate.sleep() def set_mode_callback(self, data): self.current_mode = data def set_vel(self, vel): self._vel_msg.twist.linear.x = vel.twist.linear.x self._vel_msg.twist.linear.y = vel.twist.linear.y self._vel_msg.twist.linear.z = vel.twist.linear.z#self.local_velocity.twist.linear.z def set_pose(self, pose): self._pose_msg.pose.position.x = pose.pose.position.x self._pose_msg.pose.position.y = pose.pose.position.y self._pose_msg.pose.position.z = pose.pose.position.z self._pose_msg.pose.orientation.x = pose.pose.orientation.x self._pose_msg.pose.orientation.y = pose.pose.orientation.y self._pose_msg.pose.orientation.z = pose.pose.orientation.z self._pose_msg.pose.orientation.w = pose.pose.orientation.w def set_vel_pose(self, vel_pose): print(vel_pose.pose) self._vel_pose_msg.pose.position.x = self.local_pose.pose.position.x + vel_pose.pose.position.x self._vel_pose_msg.pose.position.y = self.local_pose.pose.position.y + vel_pose.pose.position.y self._vel_pose_msg.pose.position.z = vel_pose.pose.position.z print(self._vel_pose_msg.pose) self._vel_pose_msg.pose.orientation.x = vel_pose.pose.orientation.x self._vel_pose_msg.pose.orientation.y = vel_pose.pose.orientation.y self._vel_pose_msg.pose.orientation.z = vel_pose.pose.orientation.z self._vel_pose_msg.pose.orientation.w = vel_pose.pose.orientation.w def _local_pose_callback(self, data): self.local_pose = data def _local_velocity_callback(self, data): self.local_velocity = data def set_pose_callback(self, data): self.set_pose(data) def set_velocity_callback(self, data): #self.set_vel(data) self.set_vel_pose(data) def set_x_pid(self,data): self.vController.set_x_pid(data.x, data.y, data.z) def set_y_pid(self,data): self.vController.set_y_pid(data.x, data.y, data.z) def set_z_pid(self,data): self.vController.set_z_pid(data.x, data.y, data.z) def check_distance(self): if self.current_mode.data == 'posctr': booldist = self.is_at_position(self.local_pose.pose.position, self._pose_msg.pose.position, 0.5) boolvel = self.hover_velocity() self.dist.publish(booldist and boolvel) elif self.current_mode.data == 'velctr': vel_pose_tot = PoseStamped() vel_pose_tot.pose.position.x = self._vel_pose_msg.pose.position.x vel_pose_tot.pose.position.y = self._vel_pose_msg.pose.position.y vel_pose_tot.pose.position.z = self._vel_pose_msg.pose.position.z #print("target vel_pos: {}".format(vel_pose_tot)) booldist = self.is_at_position(self.local_pose.pose.position, vel_pose_tot.pose.position, 0.2) boolvel = self.hover_velocity() self.dist.publish(booldist and boolvel) def is_at_position(self,p_current, p_desired, offset): des_pos = np.array((p_desired.x, p_desired.y, p_desired.z)) cur_pos = np.array((p_current.x, p_current.y, p_current.z)) distance = np.linalg.norm(des_pos - cur_pos) return distance < offset def hover_velocity(self): return self.local_velocity.twist.linear.x < 0.2 and self.local_velocity.twist.linear.y < 0.2 and self.local_velocity.twist.linear.z < 0.2 if __name__ == '__main__': try: position_control() except rospy.ROSInterruptException: pass