def blocked_yaw(self, yaw=120):
throttle_pid = pid_controller.PID(P=1.0,
I=0.0,
D=0.5,
Integrator_max=0.5,
Integrator_min=0.0)
throttle_pid.setPoint(self.current_alt)
"attempting to maintain alt: ", self.current_alt
time.sleep(2)
while True:
att_pub = SP.get_pub_attitude_pose(queue_size=10)
thd_pub = SP.get_pub_attitude_throttle(queue_size=10)
#while not rospy.is_shutdown():
pose = SP.PoseStamped(header=SP.Header(stamp=rospy.get_rostime()))
q = quaternion_from_euler(0, 0, 90)
pose.pose.orientation = SP.Quaternion(*q)
throt = throttle_pid.update(self.current_alt)
#if throt > 0.6:
# throt = 0.6
#if throt < 0.4:
# throt = 0.6
if self.attitude_publish or True:
att_pub.publish(pose)
thd_pub.publish(data=throt)
print "pose orientation: ", throt
def navigate(self):
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
quaternion = quaternion_from_euler(0, 0, self.yaw)
#msg.pose.orientation.x = quaternion[0]
#msg.pose.orientation.y = quaternion[1]
#msg.pose.orientation.z = quaternion[2]
#msg.pose.orientation.w = quaternion[3]
# For demo purposes we will lock yaw/heading to north.
#yaw_degrees = 180 # North
#yaw = radians(yaw_degrees)
#quaternion = quaternion_from_euler(0, 0, self.yaw)
msg.pose.orientation = SP.Quaternion(*quaternion)
self.pub.publish(msg)
rate.sleep()
def publish_orientation(self, ang=180):
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
pose = SP.PoseStamped(header=SP.Header(stamp=rospy.get_rostime()))
q = quaternion_from_euler(0, 0, ang)
pose.pose.orientation = SP.Quaternion(*q)
#thd_pub.publish(data=self.throttle_update)
#thd_pub.publish(data=0.6)
att_pub.publish(pose)
print "pose orientation: ", self.throttle_update
def publish_pose(self):
self.pub = SP.get_pub_position_local(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
msg.pose.position.x = 0.0
msg.pose.position.y = 0.0
msg.pose.position.z = 0.0
# For demo purposes we will lock yaw/heading to north.
yaw_degrees = self.yaw # North
yaw = radians(yaw_degrees)
quaternion = quaternion_from_euler(0, 0, yaw)
msg.pose.orientation = SP.Quaternion(*quaternion)
if self.publish == True:
self.pub.publish(msg)
print "publishing position"
rate.sleep()
def navigate(self):
msg = SP.PoseStamped(
header=SP.Header(
frame_id="waypoint_to_go", # 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
yaw = radians(self.yaw_degrees)
quaternion = quaternion_from_euler(0, 0, yaw)
msg.pose.orientation = SP.Quaternion(*quaternion)
self.pub.publish(msg)
rate.sleep()
def navigate(self):
""" Continuously publishes position target to the flight controller"""
rate = rospy.Rate(8) # in Hz
rate_mode_check = rospy.Rate(20)
message_pos = setpoint.PoseStamped(
header=setpoint.Header(
frame_id="base_footprint", # no matter, plugin doesn't use TF
stamp=rospy.Time.now()), # stamp should update
)
while not rospy.is_shutdown():
message_pos.pose.position.x = self.setpoint_x
message_pos.pose.position.y = self.setpoint_y
message_pos.pose.position.z = self.setpoint_z
yaw = 0
quaternion = quaternion_from_euler(0, 0, yaw)
message_pos.pose.orientation = setpoint.Quaternion(*quaternion)
self.setpoint_position.publish(message_pos)
rate.sleep()
def control_loop(self):
rate = rospy.Rate(self.rate) # 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_ref
msg.pose.position.y = self.y_ref
msg.pose.position.z = self.z_ref
# For demo purposes we will lock yaw/heading to north.
yaw_degrees = 0 # North
yaw = radians(yaw_degrees)
quaternion = quaternion_from_euler(0, 0, yaw)
msg.pose.orientation = SP.Quaternion(*quaternion)
self.pub.publish(msg)
rate.sleep()
def navigate_position(self):
msg_set_pos = SP.PoseStamped(
header=SP.Header(
frame_id="waypoint_to_go", # no matter, plugin don't use TF
stamp=rospy.Time.now()
), # stamp should update
)
while not rospy.is_shutdown():
if not self.activated:
break
msg_set_pos.pose.position.x = self.x
msg_set_pos.pose.position.y = self.y
msg_set_pos.pose.position.z = self.z
yaw = math.radians(self.yaw_degrees)
quaternion = quaternion_from_euler(0, 0, yaw)
msg_set_pos.pose.orientation = SP.Quaternion(*quaternion)
# rospy.loginfo("there is life on navigate")
self.pub_setpoint.publish(msg_set_pos)
rate.sleep()
def navigate(self):
rate = rospy.Rate(10) # 10hz
command =Twist()
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
# For demo purposes we will lock yaw/heading to north.
yaw_degrees = self.ya # North: ya=0
yaw = radians(yaw_degrees)
quaternion = quaternion_from_euler(0, 0, yaw)
msg.pose.orientation = SP.Quaternion(*quaternion)
command.angular.z = kp*yaw #yaw pose control
self.pub_cmd_vel.publish(command) #publish yaw control
self.pub_pos.publish(msg) #publish position control
rate.sleep()
