def velocity_init(self): self.vx = 0.0 self.vy = 0.0 self.vz = 0.0 self.yaw = 0.0 self.velocity_publish = False self.use_pid = True self.pid_alt = pid_controller.PID() self.pid_alt.setPoint(1.0) # publisher for mavros/setpoint_position/local self.pub_vel = SP.get_pub_velocity_cmd_vel(queue_size=10) # subscriber for mavros/local_position/local self.sub = rospy.Subscriber( mavros.get_topic('local_position', 'local'), SP.PoseStamped, self.temp) try: thread.start_new_thread(self.navigate, ()) except: fault("Error: Unable to start thread") # TODO(simon): Clean this up. self.done = False self.done_evt = threading.Event()
def init(self, _x, _y, _z): self.x = _x self.y = _y self.z = _z self.x_vel = 0 self.y_vel = 0 self.z_vel = 0 self.reaching_distance = DEFAULT_REACH_DIST self.yaw_degrees = 0 self.done_evt = threading.Event() # publisher for fuzzy (test) self.pub_fuz = rospy.Publisher('fuzzy_values', fuzzy_msg, queue_size=10) # subscriber for fuzzy (test) self.sub_fuz = rospy.Subscriber('defuzzy_values', defuzzy_msg, self.calculate_fuzzy) # publisher for reference position (ploting reasons) self.pub_refpos = rospy.Publisher('reference_pos', SP.PoseStamped, queue_size=10) # publisher for mavros/setpoint_position/local self.pub = SP.get_pub_velocity_cmd_vel(queue_size=10) # subscriber for mavros/local_position/local self.sub = rospy.Subscriber(mavros.get_topic('local_position', 'velocity_local'), SP.TwistStamped, self.velocity_meter) # publisher for smc gains self.pub_smc = rospy.Publisher('smc_values', vehicle_smc_gains_msg, queue_size=10)
def __init__(self, copter_id = "1"): self.copter_id = copter_id mavros_string = "/mavros" #rospy.init_node('velocity_goto_'+copter_id) mavros.set_namespace(mavros_string) # initialize mavros module with default namespace self.mavros_string = mavros_string self.final_alt = 0.0 self.final_pos_x = 0.0 self.final_pos_y = 0.0 self.final_vel = 0.0 self.cur_rad = 0.0 self.cur_alt = 0.0 self.cur_pos_x = 0.0 self.cur_pos_y = 0.0 self.cur_vel = 0.0 self.vx = 0.0 self.vy = 0.0 self.vz = 0.0 self.avz = 0.0 self.pose_open = [] self.alt_control = True self.override_nav = False self.reached = True self.done = False self.last_sign_dist = 0.0 # for local button handling self.click = " " self.button_sub = rospy.Subscriber("abpause_buttons", std_msgs.msg.String, self.handle_buttons) # publisher for mavros/copter*/setpoint_position/local self.pub_vel = SP.get_pub_velocity_cmd_vel(queue_size=10) # subscriber for mavros/copter*/local_position/local self.sub = rospy.Subscriber(mavros.get_topic('local_position', 'local'), SP.PoseStamped, self.temp) self.cv_bridge = CvBridge() self.image_data = open('image_data.txt','w')
def init(self, _x, _y, _z): self.x = _x self.y = _y self.z = _z self.x_vel = 0 self.y_vel = 0 self.z_vel = 0 self.yaw_degrees = 0 self.done_evt = threading.Event() # publisher for mavros/setpoint_position/local self.pub = SP.get_pub_velocity_cmd_vel(queue_size=10) # subscriber for mavros/local_position/local self.sub = rospy.Subscriber( mavros.get_topic('local_position', 'velocity_local'), SP.TwistStamped, self.velocity_meter)
def __init__(self): self.vx = 0.0 self.vy = 0.0 self.vz = 0.0 # publisher for mavros/setpoint_position/local self.pub = SP.get_pub_velocity_cmd_vel(queue_size=10) # subscriber for mavros/local_position/local self.sub = rospy.Subscriber(mavros.get_topic('local_position', 'local'), SP.PoseStamped, self.temp) try: thread.start_new_thread(self.navigate, ()) except: fault("Error: Unable to start thread") # TODO(simon): Clean this up. self.done = False self.done_evt = threading.Event()
def __init__(self, copter_id = "1"): self.copter_id = copter_id mavros_string = "/mavros/copter"+copter_id #rospy.init_node('velocity_goto_'+copter_id) mavros.set_namespace(mavros_string) # initialize mavros module with default namespace self.pid_alt = pid_controller.PID() self.mavros_string = mavros_string self.final_alt = 0.0 self.final_pos_x = 0.0 self.final_pos_y = 0.0 self.final_vel = 0.0 self.cur_rad = 0.0 self.cur_alt = 0.0 self.cur_pos_x = 0.0 self.cur_pos_y = 0.0 self.cur_vel = 0.0 self.vx = 0.0 self.vy = 0.0 self.vz = 0.0 self.pose_open = [] self.alt_control = True self.override_nav = False self.reached = True self.done = False self.last_sign_dist = 0.0 # for local button handling self.click = " " self.button_sub = rospy.Subscriber("abpause_buttons", std_msgs.msg.String, self.handle_buttons) # publisher for mavros/copter*/setpoint_position/local self.pub_vel = SP.get_pub_velocity_cmd_vel(queue_size=10) # subscriber for mavros/copter*/local_position/local self.sub = rospy.Subscriber(mavros.get_topic('local_position', 'local'), SP.PoseStamped, self.temp)
def __init__(self, XY_P, XY_I, XY_D, Z_P, Z_I, Z_D, tollerance=0.05, rate=10, sample_time=0.1, topic='/mavros/local_position/pose', name='pos_controller'): ControlPosition.__init__(self, tollerance, rate, topic, name) self.sample_time = sample_time # publisher for mavros/setpoint_velocity/cmd_vel self.pub = SP.get_pub_velocity_cmd_vel(queue_size=10) self.pidx = PID(XY_P, XY_I, XY_D) self.pidy = PID(XY_P, XY_I, XY_D) self.pidz = PID(Z_P, Z_I, Z_D) self.pidx.setSampleTime(self.sample_time) self.pidy.setSampleTime(self.sample_time) self.pidz.setSampleTime(self.sample_time) self.x = 0.0 self.y = 0.0 self.z = 0.0 try: thread.start_new_thread(self.control_loop, ()) except: fault("Error: Unable to start thread") # TODO: define as parameters of the node self.wp_radius = 0.2 self.v0 = 0.5
def publish_angular_linear_orientation(self): #att_pub = SP.get_pub_attitude_pose(queue_size=10) #thd_pub = SP.get_pub_attitude_throttle(queue_size=10) vel_pub = SP.get_pub_velocity_cmd_vel(queue_size=10) rate = rospy.Rate(10) # 10hz ## msg = SP.PoseStamped( ## header=SP.Header( ## frame_id="base_footprint", # no matter, plugin don't use TF ## stamp=rospy.Time.now()), # stamp should update ## ) while not rospy.is_shutdown(): #msg.pose.position.x = self.x #msg.pose.position.y = self.y #msg.pose.position.z = self.z twist = SP.TwistStamped(header=SP.Header(stamp=rospy.get_rostime())) twist.twist.linear = SP.Vector3(x=0, y=0, z=0) twist.twist.angular = SP.Vector3(z=-0.4) print "publishing orientation" vel_pub.publish(twist)
def velocity_init(self): self.vx = 0.0 self.vy = 0.0 self.vz = 0.0 self.yaw = 0.0 self.yaw_target = 0.0 self.velocity_publish = False self.use_pid = True self.pid_alt = pid_controller.PID() self.pid_alt.setPoint(10.0) # publisher for mavros/setpoint_position/local self.pub_vel = SP.get_pub_velocity_cmd_vel(queue_size=10) # subscriber for mavros/local_position/local self.sub = rospy.Subscriber(mavros.get_topic('local_position', 'local'), SP.PoseStamped, self.temp) try: thread.start_new_thread(self.navigate, ()) except: fault("Error: Unable to start thread") # TODO(simon): Clean this up. self.done = False self.done_evt = threading.Event()
def publish_angular_linear_orientation(self): #att_pub = SP.get_pub_attitude_pose(queue_size=10) #thd_pub = SP.get_pub_attitude_throttle(queue_size=10) vel_pub = SP.get_pub_velocity_cmd_vel(queue_size=10) rate = rospy.Rate(10) # 10hz ## msg = SP.PoseStamped( ## header=SP.Header( ## frame_id="base_footprint", # no matter, plugin don't use TF ## stamp=rospy.Time.now()), # stamp should update ## ) while not rospy.is_shutdown(): #msg.pose.position.x = self.x #msg.pose.position.y = self.y #msg.pose.position.z = self.z twist = SP.TwistStamped(header=SP.Header( stamp=rospy.get_rostime())) twist.twist.linear = SP.Vector3(x=0, y=0, z=0) twist.twist.angular = SP.Vector3(z=-0.4) print "publishing orientation" vel_pub.publish(twist)
def start_subs(self): # publisher for mavros/setpoint_position/local self.pub_vel = SP.get_pub_velocity_cmd_vel(queue_size=10) # subscriber for mavros/local_position/local self.sub = rospy.Subscriber(mavros.get_topic('local_position', 'local'), SP.PoseStamped, self.temp)
def velocity_call(): print("He!") rospy.loginfo("MAV-Teleop: Velocity setpoint control type.") mode = rospy.ServiceProxy("mavros/set_mode", SetMode) global pub pub = SP.get_pub_velocity_cmd_vel(queue_size=10) global wait global algo_ctrl while 1: fd = sys.stdin.fileno() old_settings = termios.tcgetattr(fd) try: tty.setraw(sys.stdin.fileno()) ch = sys.stdin.read(1) finally: termios.tcsetattr(fd, termios.TCSADRAIN, old_settings) x = ch wait = True #pub.publish(twist_obj(0.0, 0.0, 0.0, 0.0, 0.0, 0.0)) if (x == chr(27)): #import tty #required this import here tty.tcsetattr(fd, termios.TCSADRAIN, old_settings) os._exit(0) global horVel global rotVel global verVel horVel = 0.5 rotVel = 1.0 verVel = 0.5 print('\n'+"Keypressed-->"+x+'\n') #pub.publish(twist_obj(0.0, 0.0, 0.0, 0.0, 0.0, 0.0)) if (x == 'a'): pub.publish(twist_obj(0.0, 0.0, 0.0, 0.0, 0.0, -1*rotVel)) elif (x == 'd'): pub.publish(twist_obj(0.0, 0.0, 0.0, 0.0, 0.0, 1*rotVel)) elif (x == 'w'): pub.publish(twist_obj(0.0, 0.0, 1*verVel, 0.0, 0.0, 0.0)) elif (x == 's'): pub.publish(twist_obj(0.0, 0.0, -1*verVel, 0.0, 0.0, 0.0)) elif (x == 'u'): pub.publish(twist_obj(1*horVel, 1*horVel, 0.0, 0.0, 0.0, 0.0)) elif (x == 'o'): pub.publish(twist_obj(1*horVel, -1*horVel, 0.0, 0.0, 0.0, 0.0)) elif (x == 'i'): pub.publish(twist_obj(1*horVel, 0.0, 0.0, 0.0, 0.0, 0.0)) elif (x == 'k'): pub.publish(twist_obj(-1*horVel, 0.0, 0.0, 0.0, 0.0, 0.0)) elif (x == 'j'): pub.publish(twist_obj(0.0, 1*horVel, 0.0, 0.0, 0.0, 0.0)) elif (x == 'l'): pub.publish(twist_obj(0.0, -1*horVel, 0.0, 0.0, 0.0, 0.0)) elif (x == '.'): land(args) elif (x == ','): # takeoff(args) resp = mode(0,"AUTO.TAKEOFF") pub.publish(twist_obj(0.0, 0.0, 0.0, 0.0, 0.0, 0.0)) elif (x == 'm'): arm(args,True) resp = mode(0,"OFFBOARD") # print "setmode send ok %s" % resp.success elif (x == '/'): arm(args, False) elif (x == 'z'): algo_ctrl = not algo_ctrl print(algo_ctrl) wait = False # px4_control() else: pub.publish(twist_obj(0.0, 0.0, 0.0, 0.0, 0.0, 0.0))
ekf_pose.header.stamp = ekf_path.header.stamp ekf_path.poses.append(ekf_pose) # mpc_pose.header.seq = ekf_path.header.seq + 1 mpc_path.header.frame_id = "local_origin" mpc_path.header.stamp = rospy.Time.now() # mpc_pose.header.stamp = mpc_path.header.stamp mpc_path.poses = mpc_pose_array cont = cont + 1 xAnt = pose.pose.orientation.x yAnt = pose.pose.position.y pub4.publish(path) pub5.publish(ekf_path) pub6.publish(mpc_path) if cont > max_append and len(path.poses) != 0 and len(ekf_path.poses): path.poses.pop(0) ekf_path.poses.pop(0) br.sendTransform((pos.x, pos.y, pos.z), (quat.x, quat.y, quat.z, quat.w), rospy.Time.now(), "base_link", "local_origin") br2.sendTransform((0, 0, 0), (0, 0, 0, 1), rospy.Time.now(), "fcu", "local_origin") if __name__ == "__main__": mavros.set_namespace("/mavros") pub1 = SP.get_pub_velocity_cmd_vel(queue_size=3) path = Path() np.set_printoptions(precision=None, threshold=None, edgeitems=None, linewidth=1000, suppress=None, nanstr=None, infstr=None, formatter=None) main()