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()