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