def hover_rec(self, time_flying): recursions = self.calculate_recursions(time_flying) print(recursions) for i in range(recursions): print(i) print(self.down_sensor_distance) if self.beh_type == 'HOVER' and (self.hover_sensor_altitude_max >= self.down_sensor_distance >= self.hover_sensor_altitude_min): print("The drone is hovering") self.beh_type = 'HOVER' target_raw_attitude = AttitudeTarget() target_raw_attitude.header.stamp = rospy.Time.now() target_raw_attitude.orientation = self.imu.orientation target_raw_attitude.body_rate.x = 0 # ROLL_RATE target_raw_attitude.body_rate.y = 0 # PITCH_RATE target_raw_attitude.body_rate.z = 0 # YAW_RATE thrust = self.hover_thrust target_raw_attitude.thrust = thrust self.attitude_target_pub.publish(target_raw_attitude) time.sleep(self.Time_between_messages ) #was 0.005 (now 50hz ,500loops) elif self.beh_type == 'HOVER' and ( self.hover_sensor_altitude_max <= self.down_sensor_distance): # We have to go down print("Recovering hover position - going down") self.beh_type = 'HOVER' target_raw_attitude = AttitudeTarget() target_raw_attitude.header.stamp = rospy.Time.now() target_raw_attitude.orientation = self.imu.orientation target_raw_attitude.body_rate.x = 0 # ROLL_RATE target_raw_attitude.body_rate.y = 0 # PITCH_RATE target_raw_attitude.body_rate.z = 0 # YAW_RATE thrust = self.hover_thrust target_raw_attitude.thrust = thrust - self.Deaccumulating_thrust self.attitude_target_pub.publish(target_raw_attitude) time.sleep(self.Time_between_messages ) # was 0.005 (now 50hz ,500 loops ,5sec) elif self.beh_type == 'HOVER' and ( self.down_sensor_distance <= self.hover_sensor_altitude_min): # We have to go up print("Recovering hover position - going up") self.beh_type = 'HOVER' target_raw_attitude = AttitudeTarget() target_raw_attitude.header.stamp = rospy.Time.now() target_raw_attitude.orientation = self.imu.orientation target_raw_attitude.body_rate.x = 0 # ROLL_RATE target_raw_attitude.body_rate.y = 0 # PITCH_RATE target_raw_attitude.body_rate.z = 0 # YAW_RATE thrust = self.hover_thrust target_raw_attitude.thrust = thrust + self.Deaccumulating_thrust self.attitude_target_pub.publish(target_raw_attitude) time.sleep(self.Time_between_messages ) # was 0.005 (now 50hz ,500 loops ,5sec) print("time of hovering has ended") self.beh_type = "LANDING" self.landing_rec()
def secure_landing_phase_rec(self): # Secure landing part - last cm self.beh_type = "LANDING" recursions = self.calculate_recursions(self.Secure_time_landing) thrust = self.hover_thrust for i in range(recursions): if thrust <= 0: break elif i < 200: print("Secure hover before landing") target_raw_attitude = AttitudeTarget() target_raw_attitude.header.stamp = rospy.Time.now() target_raw_attitude.orientation = self.imu.orientation target_raw_attitude.body_rate.x = 0 # ROLL_RATE target_raw_attitude.body_rate.y = 0 # PITCH_RATE target_raw_attitude.body_rate.z = 0 # YAW_RATE thrust = self.hover_thrust target_raw_attitude.thrust = thrust self.attitude_target_pub.publish(target_raw_attitude) time.sleep(self.Time_between_messages ) # was 0.005 (now 50hz ,500 loops ,5sec) elif i < 700: print("Smooth landing") target_raw_attitude = AttitudeTarget() target_raw_attitude.header.stamp = rospy.Time.now() target_raw_attitude.orientation = self.imu.orientation target_raw_attitude.body_rate.x = 0 # ROLL_RATE target_raw_attitude.body_rate.y = 0 # PITCH_RATE target_raw_attitude.body_rate.z = 0 # YAW_RATE thrust = self.hover_thrust - self.Deaccumulating_thrust target_raw_attitude.thrust = thrust self.attitude_target_pub.publish(target_raw_attitude) time.sleep(self.Time_between_messages ) # was 0.005 (now 50hz ,500 loops ,5sec) else: print("Landing") target_raw_attitude = AttitudeTarget() target_raw_attitude.header.stamp = rospy.Time.now() target_raw_attitude.orientation = self.imu.orientation target_raw_attitude.body_rate.x = 0 # ROLL_RATE target_raw_attitude.body_rate.y = 0 # PITCH_RATE target_raw_attitude.body_rate.z = 0 # YAW_RATE target_raw_attitude.thrust = thrust thrust = thrust - self.accumulating_thrust self.attitude_target_pub.publish(target_raw_attitude) time.sleep(self.Time_between_messages ) # was 0.005 (now 50hz ,500 loops ,5sec) self.disarm()
def main1(): global setpoint # thrust_pub = rospy.Publisher('/mavros/setpoint_attitude/thrust',Thrust,queue_size=10) # thrust = Thrust() attitude_pub = rospy.Publisher('/mavros/setpoint_raw/attitude',AttitudeTarget,queue_size=10) point_pub = rospy.Publisher('/waypoint',PoseStamped,queue_size=10) attitude = AttitudeTarget() attitude.type_mask = 3 attitude.header = msg.header # attitude.header.frame_id = 'FRAME_LOCAL_NED' # quaternion = tf.transformations.quaternion_from_euler(msg.roll,msg.pitch, 0) # attitude.orientation.x = quaternion[0] # attitude.orientation.y = quaternion[1] # attitude.orientation.z = quaternion[2] # attitude.orientation.w = quaternion[3] attitude.orientation = Quaternion(*tf_conversions.transformations.quaternion_from_euler(msg.roll, msg.pitch, 0)) attitude.body_rate.z = msg.yaw_rate t = msg.thrust.z/100 if t>1: t=1 elif t<-1: t=-1 attitude.thrust = (t+1)/2 attitude_pub.publish(attitude) point_pub.publish(setpoint)
def imu_cb(self, data): q = [ data.orientation.x, data.orientation.y, data.orientation.z, data.orientation.w ] if self.imu_set: q = self.imu_q euler = euler_from_quaternion(q) q_step = quaternion_from_euler(0.0, 0.0, np.deg2rad(self.step_size)) new_q = quaternion_multiply(q, q_step) new_euler = euler_from_quaternion(new_q) self.imu_set = True self.imu_q = q print "Current Yaw: " + str(np.rad2deg(euler[2])) print "New Yaw: " + str(np.rad2deg(new_euler[2])) msg = AttitudeTarget() msg.header.stamp = rospy.Time.now() msg.thrust = self.thrust msg.orientation.x = new_q[0] msg.orientation.y = new_q[1] msg.orientation.z = new_q[2] msg.orientation.w = new_q[3] self.setpoint_pub.publish(msg)
def callback(data): global pub, trim_x, trim_y, trim_z cmd_ = AttitudeTarget() cmd_.body_rate.x = valmap(-data.axes[0], -1, 1, -100, 100) + trim_x cmd_.body_rate.y = valmap(data.axes[1], -1, 1, -100, 100) + trim_y cmd_.body_rate.z = valmap(-data.axes[2], -1, 1, -200, 200) + trim_z #cmd_.body_rate.x = valmap(-data.axes[0], -1, 1, -1250, 1250) #cmd_.body_rate.y = valmap(data.axes[1], -1, 1, -1250, 1250) #cmd_.body_rate.z = valmap(data.axes[2], -1, 1, -6000, 6000) cmd_.thrust = valmap(data.axes[3], -1, 1, 0, 700) if data.buttons[11]: trim_x += 1 rospy.loginfo("trim_x: " + str(trim_x)) if data.buttons[10]: trim_x -= 1 rospy.loginfo("trim_x: " + str(trim_x)) if data.buttons[9]: trim_y += 1 rospy.loginfo("trim_y: " + str(trim_y)) if data.buttons[8]: trim_y -= 1 rospy.loginfo("trim_y: " + str(trim_y)) if data.buttons[7]: trim_z += 1 rospy.loginfo("trim_z: " + str(trim_z)) if data.buttons[6]: trim_z -= 1 rospy.loginfo("trim_z: " + str(trim_z)) pub.publish(cmd_)
def callback(self, rc): #rospy.loginfo('{} {} {} {}'.format(rc.channels[0], rc.channels[1], rc.channels[2], rc.channels[3])) cmd = AttitudeTarget() Ts = (rospy.Time.now() - self.previous_time).to_sec() self.previous_time = rospy.Time.now() cmd.header.stamp = rospy.Time.now() cmd.type_mask = AttitudeTarget.IGNORE_ROLL_RATE + AttitudeTarget.IGNORE_PITCH_RATE #ROS use ENU convention roll = .6 * (rc.channels[1] - 1492.) / 500. pitch = -.6 * (rc.channels[2] - 1493.) / 500. yawrate = -3. * (rc.channels[3] - 1498.) / 500. self.yaw = self.yaw + yawrate * Ts thrust = (rc.channels[0] - 1000.) / 1200. rospy.loginfo( 'Ts = {Ts:.3f} r = {r:.2f} p = {p:.2f} y = {y:.2f} t = {t:.2f}'. format(Ts=Ts, r=roll, p=pitch, y=self.yaw, t=thrust)) q = quaternion_from_euler(roll, pitch, self.yaw) cmd.orientation.x = q[0] cmd.orientation.y = q[1] cmd.orientation.z = q[2] cmd.orientation.w = q[3] cmd.body_rate.x = 0. cmd.body_rate.y = 0. cmd.body_rate.z = yawrate cmd.thrust = thrust self.pub.publish(cmd)
def set_ang_vel_thrust(self, ang_vel, thrust, freq): """ Offboard method that sends angular velocity and thrust references to the PX4 autopilot of the the drone. Makes convertions from the local NED frame adopted for the ISR Flying Arena to the local ENU frame used by ROS. Converts the thrust from newtons to a normalized value between 0 and 1 through the mathematical expression of the thrust curve of the vehicle. Parameters ---------- ang_vel : np.array of floats with shape (3,1) Desired angular velocity for the drone. thrust : float Desired thrust value in newtons. freq : float Topic publishing frequency, in Hz. """ pub = rospy.Publisher(self.info.drone_ns + '/mavros/setpoint_raw/attitude', AttitudeTarget, queue_size=1) msg = AttitudeTarget() msg.header = Header() msg.header.stamp = rospy.Time.now() msg.type_mask = 128 msg.body_rate.x, msg.body_rate.y, msg.body_rate.z = ned_to_enu(ang_vel) msg.thrust = normalize_thrust(thrust, self.info.thrust_curve, norm(self.ekf.vel)) pub.publish(msg) rate = rospy.Rate(freq) rate.sleep()
def track_and_trajectory_dock_control(self,des_trajectory_point, curr_odom, yaw_des, time_now, docker): if docker == self.num: pass else: return Rot_des = np.matrix(np.zeros((4, 4))) Rot_des[3, 3] = 1 attitude_des = AttitudeTarget() thrust = Thrust() des_acc = Vector3() if self.A_init_flag: thrust.header = curr_odom.header attitude_des.header = curr_odom.header dt = time_now - self.time_prev ex = des_trajectory_point.position.x - curr_odom.position.x ey = des_trajectory_point.position.y - curr_odom.position.y ez = des_trajectory_point.position.z - curr_odom.position.z evx = des_trajectory_point.velocity.x - curr_odom.velocity.x evy = des_trajectory_point.velocity.y - curr_odom.velocity.y evz = des_trajectory_point.velocity.z - curr_odom.velocity.z self.intx = self.intx + ex * dt self.inty = self.inty + ey * dt self.intz = self.intz + ez * dt des_acc.x = des_trajectory_point.acceleration.x + self.Kp_track.x * ex + self.Kd_track.x * evx + self.Ki_track.x * self.intx des_acc.y = des_trajectory_point.acceleration.y + self.Kp_track.y * ey + self.Kd_track.y * evy + self.Ki_track.y * self.inty des_acc.z = des_trajectory_point.acceleration.z + self.Kp_track.z * ez + self.Kd_track.z * evz + self.Ki_track.z * self.intz R_waitmod_w = np.matrix([[np.cos(self.tag_angle),-np.sin(self.tag_angle),0],[np.sin(self.tag_angle),np.cos(self.tag_angle),0],[0,0,1]]).transpose() curr_quaternion = [curr_odom.orientation.x, curr_odom.orientation.y, curr_odom.orientation.z, curr_odom.orientation.w] H_curr = tf.transformations.quaternion_matrix(curr_quaternion) Rot_curr = np.matrix(H_curr[:3, :3]) des_acc_relative = R_waitmod_w*np.matrix([[des_acc.x], [des_acc.y], [des_acc.z]]) Force_des = np.matrix([[0], [0], [self.m * self.g]])+self.m * des_acc_relative Force_des_body = Rot_curr * Force_des thrust.thrust = Force_des_body[2] Rot_des[:3, 2] = np.matrix(Force_des / LA.norm(Force_des)) x_body_in_world = np.matrix([[np.cos(des_trajectory_point.yaw)], [np.sin(des_trajectory_point.yaw)], [0]]) y_body_in_world = np.cross(Rot_des[:3, 2], x_body_in_world, axis=0) Rot_des[:3, 1] = np.matrix(y_body_in_world / LA.norm(y_body_in_world)) Rot_des[:3, 0] = np.matrix(np.cross(Rot_des[:3, 1], Rot_des[:3, 2], axis=0)) quad_des = tf.transformations.quaternion_from_matrix(Rot_des) attitude_des.orientation.x = quad_des[0] attitude_des.orientation.y = quad_des[1] attitude_des.orientation.z = quad_des[2] attitude_des.orientation.w = quad_des[3] attitude_des.thrust = thrust.thrust self.mavros_attitude_pub.publish(attitude_des) self.mavros_thrust_pub.publish(thrust) self.time_prev = time_now
def construct_target_attitude(self, body_x=0, body_y=0, body_z=0, thrust=0, roll_angle=0, pitch_angle=0, yaw_angle=0): target_raw_attitude = AttitudeTarget( ) # We will fill the following message with our values: http://docs.ros.org/api/mavros_msgs/html/msg/PositionTarget.html target_raw_attitude.header.stamp = rospy.Time.now() #target_raw_attitude.orientation = self.imu.orientation q = self.to_quaternion( roll_angle + self.angle_roll_left_right_trim, pitch_angle + self.angle_pitch_forward_back_trim, yaw_angle) #q = self.to_quaternion(roll_angle, pitch_angle, yaw_angle) target_raw_attitude.orientation.w = q[0] target_raw_attitude.orientation.x = q[1] target_raw_attitude.orientation.y = q[2] target_raw_attitude.orientation.z = q[3] target_raw_attitude.body_rate.x = body_x # ROLL_RATE target_raw_attitude.body_rate.y = body_y # PITCH_RATE target_raw_attitude.body_rate.z = body_z # YAW_RATE target_raw_attitude.thrust = thrust self.attitude_target_pub.publish(target_raw_attitude) time.sleep(self.Time_between_messages)
def set_att_thrust(self, att, att_type, thrust, freq): """ Offboard method that sends attitude and thrust references to the PX4 autopilot of the the vehicle. Makes convertions from the local NED frame adopted for the ISR Flying Arena to the local ENU frame used by ROS. Converts the thrust from newtons to a normalized value between 0 and 1 through mathematical expression of the thrust curve of the vehicle. Parameters ---------- att : np.array of floats with shape (3,1) or with shape (4,1) Desired attitude for the vehicle, expressed in euler angles or in a quaternion. att_type : str Must be equal to either 'euler' or 'quaternion'. Specifies the format of the desired attitude. thrust : float Desired thrust value in newtons. freq : float Topic publishing frequency, in Hz. """ pub = rospy.Publisher(self.info.drone_ns + '/mavros/setpoint_raw/attitude', AttitudeTarget, queue_size=1) msg = AttitudeTarget() msg.header = Header() msg.header.stamp = rospy.Time.now() if att_type == 'euler': msg.orientation = Quaternion(*quaternion_from_euler( *ned_to_enu(att))) elif att_type == 'quaternion': msg.orientation = Quaternion(*SI_quaternion_to_ROS_quaternion(att)) msg.thrust = normalize_thrust(thrust, self.info.thrust_curve, norm(self.ekf.vel)) pub.publish(msg) rate = rospy.Rate(freq) rate.sleep()
def update_attitude(self): # Create message msg = AttitudeTarget(header=Header(stamp=rospy.Time.now())) # Ignore all but thrust msg.type_mask = AttitudeTarget.IGNORE_ROLL_RATE | AttitudeTarget.IGNORE_PITCH_RATE | AttitudeTarget.IGNORE_YAW_RATE | AttitudeTarget.IGNORE_ATTITUDE # # Set attitude # msg.orientation.x = 0 # msg.orientation.y = 0 # msg.orientation.z = 0 # msg.orientation.w = 0 # # Set rates # msg.body_rate.x = 0.1 # msg.body_rate.y = 0.1 # msg.body_rate.z = 0.1 # Set velocity msg.thrust = self._thrust # Navigate until location is reached while not self._done.is_set() and not rospy.is_shutdown(): # Publish self._rawatt_pub.publish(msg) # Ensure proper communication rate self._rate.sleep()
def att_controller(self, r=0, p=0, y=0, z=2): if hasattr(self, "roll_zero"): r += self.roll_zero if hasattr(self, "pitch_zero"): p + self.pitch_zero att = AttitudeTarget() q = tf.transformations.quaternion_from_euler(r, p, y) att.orientation.x = q[0] att.orientation.y = q[1] att.orientation.z = q[2] att.orientation.w = q[3] rx, ry, rz, r, p, y = self.parse_local_position(mode="e") vx, vy, vz, wx, wy, wz = self.parse_velocity() try: param = 1.0 / (np.cos(r) * np.cos(p)) except Exception as e: param = 1 finally: vzt = self.pidz.step(z - rz) att.thrust = (self.pidzv.step(vzt - vz) + 0.57) * param # for plot only.. self.thrust = att.thrust self.pidzv_out = self.pidzv.out self.pidz_out = self.pidz.out att.header = Header() att.header.frame_id = "map" att.header.stamp = rospy.Time.now() self.att_setpoint_pub.publish(att)
def test_control(self): self.toggle_arm(True) #self.takeoff(1.0) self.set_offboard_mode() self.move_to(0,0,5) #sleep(2) ''' self.move_to(-2,2,3.3) sleep(2) self.move_to(0,4,3.3) sleep(2) self.move_to(2,2,3.3) sleep(2)''' ############ print('Starting') sleep(5) rate = rospy.Rate(20) sr = AttitudeTarget() sr.type_mask = 135 sr.thrust = 0.9 for i in range(20): print('stg 1') self.publish_attitude_thrust.publish(sr) rate.sleep() print('Stage 1 done') sr.type_mask = 134 sr.body_rate.x = 1500.0 sr.body_rate.y = 0.0 sr.body_rate.z = 0.0 sr.thrust = 0.5 for i in range(20): print('stg 2') self.publish_attitude_thrust.publish(sr) rate.sleep() print('Stage 2 done') sr.type_mask = 134 sr.body_rate.x = -1500.0 sr.body_rate.y = 0.0 sr.body_rate.z = 0.0 sr.thrust = 0.5 for i in range(5): print('final') self.publish_attitude_thrust.publish(sr) rate.sleep() print('Roll Complete!!')
def construct_target_attitude(self, body_x = 0, body_y = 0, body_z = 0, thrust=0): target_raw_attitude = AttitudeTarget() # We will fill the following message with our values: http://docs.ros.org/api/mavros_msgs/html/msg/PositionTarget.html target_raw_attitude.header.stamp = rospy.Time.now() target_raw_attitude.orientation = self.imu.orientation target_raw_attitude.body_rate.x = body_x # ROLL_RATE target_raw_attitude.body_rate.y = body_y # PITCH_RATE target_raw_attitude.body_rate.z = body_z # YAW_RATE target_raw_attitude.thrust = thrust self.attitude_target_pub.publish(target_raw_attitude)
def set_attitude(self, r, p, y, z, mode="height"): att = AttitudeTarget() q = tf.transformations.quaternion_from_euler(r, p, y) att.orientation.x = q[0] att.orientation.y = q[1] att.orientation.z = q[2] att.orientation.w = q[3] rx, ry, rz, qx, qy, qz, qw = self.get_uav_pos() if mode == "height": try: rpy = tf.transformations.euler_from_quaternion( (qx, qy, qz, qw)) param = 1.0 / (np.cos(rpy[0]) * np.cos(rpy[1])) except: param = 1 finally: att.thrust = self.pidz.step(z - rz) * param else: att.thrust = z self.att_setpoint_pub.publish(att)
def thrust_recursion(self, thrust): if self.status_thrust == "up": if (thrust >= 1): print ("the thrust has reached its desired point") print ("the motors should start slowing down...") target_raw_attitude = AttitudeTarget() target_raw_attitude.header.stamp = rospy.Time.now() target_raw_attitude.type_mask = AttitudeTarget.IGNORE_ROLL_RATE + AttitudeTarget.IGNORE_PITCH_RATE + AttitudeTarget.IGNORE_YAW_RATE \ + AttitudeTarget.IGNORE_ATTITUDE target_raw_attitude.thrust = 1 self.attitude_target_pub.publish(target_raw_attitude) time.sleep(0.1) self.status_thrust = "slow down" return self.thrust_recursion(target_raw_attitude.thrust) else: target_raw_attitude = AttitudeTarget() target_raw_attitude.header.stamp = rospy.Time.now() target_raw_attitude.type_mask = AttitudeTarget.IGNORE_ROLL_RATE + AttitudeTarget.IGNORE_PITCH_RATE + AttitudeTarget.IGNORE_YAW_RATE \ + AttitudeTarget.IGNORE_ATTITUDE target_raw_attitude.thrust = thrust + 0.003 self.attitude_target_pub.publish(target_raw_attitude) time.sleep(0.1) return self.thrust_recursion(target_raw_attitude.thrust) elif self.status_thrust == "slow down": if (thrust <= 0): print ("the motors should stop") self.status_thrust = "stop" return True else: target_raw_attitude = AttitudeTarget() target_raw_attitude.header.stamp = rospy.Time.now() target_raw_attitude.type_mask = AttitudeTarget.IGNORE_ROLL_RATE + AttitudeTarget.IGNORE_PITCH_RATE + AttitudeTarget.IGNORE_YAW_RATE \ + AttitudeTarget.IGNORE_ATTITUDE target_raw_attitude.thrust = thrust - 0.003 self.attitude_target_pub.publish(target_raw_attitude) time.sleep(0.1) return self.thrust_recursion(target_raw_attitude.thrust)
def landing_rec(self, thrust, beh_type, time_flying): if (time_flying <= 0 or self.down_sensor_distance <= self.landing_sensor_altitude_min) and beh_type == "LANDING": print("Entering secure landing") return self.secure_landing_phase_rec(thrust, beh_type, self.Secure_time_landing) elif thrust > self.Landing_thrust: #and beh_type == "HOVER": print("Landing the drone down slowly") target_raw_attitude = AttitudeTarget() target_raw_attitude.header.stamp = rospy.Time.now() target_raw_attitude.orientation = self.imu.orientation target_raw_attitude.body_rate.x = 0 # ROLL_RATE target_raw_attitude.body_rate.y = 0 # PITCH_RATE target_raw_attitude.body_rate.z = 0 # YAW_RATE target_raw_attitude.thrust = thrust thrust = thrust - self.accumulating_thrust self.attitude_target_pub.publish(target_raw_attitude) time_flying = time_flying - self.Time_between_messages time.sleep(self.Time_between_messages ) # was 0.005 (now 50hz ,500 loops ,5sec) return self.landing_rec( thrust, beh_type, time_flying) #bublishing a constant parameter "not elif thrust <= self.Landing_thrust: #and beh_type == "HOVER": print("Landing the drone down slowly") target_raw_attitude = AttitudeTarget() target_raw_attitude.header.stamp = rospy.Time.now() target_raw_attitude.orientation = self.imu.orientation target_raw_attitude.body_rate.x = 0 # ROLL_RATE target_raw_attitude.body_rate.y = 0 # PITCH_RATE target_raw_attitude.body_rate.z = 0 # YAW_RATE target_raw_attitude.thrust = self.Landing_thrust thrust = self.Landing_thrust self.attitude_target_pub.publish(target_raw_attitude) time_flying = time_flying - self.Time_between_messages time.sleep(self.Time_between_messages ) # was 0.005 (now 50hz ,500 loops ,5sec) return self.landing_rec( thrust, beh_type, time_flying ) #bublishing a constant parameter "not updating thrust argument"
def send_commands(self, event): # Create the AttitudeTarget command message. command_msg = AttitudeTarget() # Fill out the message. command_msg.header.stamp = rospy.get_rostime() command_msg.type_mask = self.ignore_mask command_msg.thrust = self.thrust_val # Publish the message. self.command_pub.publish(command_msg)
def construct_target_attitude(self, body_x = 0, body_y = 0, body_z = 0, thrust = 0.2): target_raw_attitude = AttitudeTarget() # We will fill the following message with our values: http://docs.ros.org/api/mavros_msgs/html/msg/PositionTarget.html target_raw_attitude.header.stamp = rospy.Time.now() #target_raw_attitude.orientation. = self.imu.orientation target_raw_attitude.type_mask = AttitudeTarget.IGNORE_ROLL_RATE + AttitudeTarget.IGNORE_PITCH_RATE + AttitudeTarget.IGNORE_YAW_RATE \ + AttitudeTarget.IGNORE_ATTITUDE #target_raw_attitude.body_rate.x = body_x # ROLL_RATE #target_raw_attitude.body_rate.y = body_y # PITCH_RATE #target_raw_attitude.body_rate.z = body_z # YAW_RATE target_raw_attitude.thrust = thrust return target_raw_attitude
def lift_off_rec(self, thrust, beh_type, time_flying): if (time_flying <= 0 or self.down_sensor_distance <= self.hover_sensor_altitude_min) and beh_type == "TAKE OFF": print("time of lift off has ended") beh_type = "HOVER" return self.hover_rec(thrust, beh_type, self.Hover_time) elif beh_type == "TAKE OFF" and thrust >= self.Liftoff_thrust and self.down_sensor_distance <= self.hover_sensor_altitude_min: print("The drone is lifting off at constant thrust") target_raw_attitude = AttitudeTarget() target_raw_attitude.header.stamp = rospy.Time.now() target_raw_attitude.orientation = self.imu.orientation target_raw_attitude.body_rate.x = 0 # ROLL_RATE target_raw_attitude.body_rate.y = 0 # PITCH_RATE target_raw_attitude.body_rate.z = 0 # YAW_RATE thrust = self.Liftoff_thrust target_raw_attitude.thrust = thrust self.attitude_target_pub.publish(target_raw_attitude) time.sleep( self.Time_between_messages) #was 0.005 (now 50hz ,500loops) time_flying = time_flying - self.Time_between_messages return self.lift_off_rec(target_raw_attitude.thrust, beh_type, time_flying) elif beh_type == "TAKE OFF": print("Lifting the drone up slowly") target_raw_attitude = AttitudeTarget() target_raw_attitude.header.stamp = rospy.Time.now() target_raw_attitude.orientation = self.imu.orientation target_raw_attitude.body_rate.x = 0 # ROLL_RATE target_raw_attitude.body_rate.y = 0 # PITCH_RATE target_raw_attitude.body_rate.z = 0 # YAW_RATE target_raw_attitude.thrust = thrust + self.accumulating_thrust self.attitude_target_pub.publish(target_raw_attitude) time_flying = time_flying - self.Time_between_messages time.sleep(self.Time_between_messages ) # was 0.005 (now 50hz ,500 loops ,5sec) return self.lift_off_rec(target_raw_attitude.thrust, beh_type, time_flying)
def landing_rec(self): # Landing phase self.beh_type = "LANDING" recursions = self.calculate_recursions(self.Max_time_landing) print(recursions) thrust = self.hover_thrust for i in range(recursions): print self.down_sensor_distance if self.down_sensor_distance <= self.landing_sensor_altitude_min: break elif thrust > self.Landing_thrust: #and beh_type == "HOVER": print("Landing the drone down slowly") target_raw_attitude = AttitudeTarget() target_raw_attitude.header.stamp = rospy.Time.now() target_raw_attitude.orientation = self.imu.orientation target_raw_attitude.body_rate.x = 0 # ROLL_RATE target_raw_attitude.body_rate.y = 0 # PITCH_RATE target_raw_attitude.body_rate.z = 0 # YAW_RATE target_raw_attitude.thrust = thrust thrust = thrust - self.accumulating_thrust self.attitude_target_pub.publish(target_raw_attitude) time.sleep(self.Time_between_messages ) # was 0.005 (now 50hz ,500 loops ,5sec) elif thrust <= self.Landing_thrust: #and beh_type == "HOVER": print("Landing the drone down slowly") target_raw_attitude = AttitudeTarget() target_raw_attitude.header.stamp = rospy.Time.now() target_raw_attitude.orientation = self.imu.orientation target_raw_attitude.body_rate.x = 0 # ROLL_RATE target_raw_attitude.body_rate.y = 0 # PITCH_RATE target_raw_attitude.body_rate.z = 0 # YAW_RATE target_raw_attitude.thrust = self.Landing_thrust thrust = self.Landing_thrust self.attitude_target_pub.publish(target_raw_attitude) time.sleep(self.Time_between_messages ) # was 0.005 (now 50hz ,500 loops ,5sec) self.secure_landing_phase_rec2()
def thrust_recursion(self, thrust): if (thrust >= 1): print("the thrust has reached its desired point") return True else: target_raw_attitude = AttitudeTarget() target_raw_attitude.header.stamp = rospy.Time.now() target_raw_attitude.type_mask = AttitudeTarget.IGNORE_ROLL_RATE + AttitudeTarget.IGNORE_PITCH_RATE + AttitudeTarget.IGNORE_YAW_RATE \ + AttitudeTarget.IGNORE_ATTITUDE target_raw_attitude.thrust = thrust + 0.003 self.attitude_target_pub.publish(target_raw_attitude) time.sleep(0.1) return self.thrust_recursion(target_raw_attitude.thrust)
def get_message(sp_att): msg = AttitudeTarget() msg.header.stamp = rospy.get_rostime() msg.type_mask = AT.IGNORE_ROLL_RATE + AT.IGNORE_PITCH_RATE + AT.IGNORE_YAW_RATE q = quaternion_from_euler(sp_att[0], sp_att[1], sp_att[2]) msg.orientation.x = q[0] msg.orientation.y = q[1] msg.orientation.z = q[2] msg.orientation.w = q[3] msg.thrust = sp_att[3] return msg
def send_attitude_command(self): command_msg = AttitudeTarget() command_msg.header.stamp = rospy.get_rostime() command_msg.type_mask = self.attitude_mask command_msg.body_rate.x = 0.0 command_msg.body_rate.y = 0.0 command_msg.body_rate.z = 0.0 command_msg.thrust = self.thrust_val # Publish. self.attitude_pub_mavros.publish(command_msg)
def pos_control(self,des_trajectory_point,curr_pose, curr_vel, yaw_des, time_now): Rot_des = np.matrix(np.zeros((4, 4))) Rot_des[3, 3] = 1 attitude_des = AttitudeTarget() thrust = Thrust() des_acc = Vector3() if self.A_init_flag: thrust.header = curr_pose.header attitude_des.header = curr_pose.header dt = time_now - self.time_prev ex = des_trajectory_point.position.x - curr_pose.pose.position.x ey = des_trajectory_point.position.y - curr_pose.pose.position.y ez = des_trajectory_point.position.z - curr_pose.pose.position.z evx = des_trajectory_point.velocity.x - curr_vel.twist.linear.x evy = des_trajectory_point.velocity.y - curr_vel.twist.linear.y evz = des_trajectory_point.velocity.z - curr_vel.twist.linear.z self.intx = self.intx + ex * dt self.inty = self.inty + ey * dt self.intz = self.intz + ez * dt des_acc.x = des_trajectory_point.acceleration.x + self.Kp.x * ex + self.Kd.x * evx + self.ki.x * self.intx des_acc.y = des_trajectory_point.acceleration.y + self.Kp.y * ey + self.Kd.y * evy + self.ki.y * self.inty des_acc.z = des_trajectory_point.acceleration.z + self.Kp.z * ez + self.Kd.z * evz + self.ki.z * self.intz curr_quaternion = [curr_pose.pose.orientation.x, curr_pose.pose.orientation.y, curr_pose.pose.orientation.z, curr_pose.pose.orientation.w] H_curr = tf.transformations.quaternion_matrix(curr_quaternion) Rot_curr = np.matrix(H_curr[:3, :3]) Force_des = np.matrix([[0], [0], [self.m * self.g]])+self.m * np.matrix([[des_acc.x], [des_acc.y], [des_acc.z]]) Force_des_body = Rot_curr * Force_des thrust.thrust = Force_des_body[2] Rot_des[:3, 2] = np.matrix(Force_des / LA.norm(Force_des)) x_body_in_world = np.matrix([[np.cos(des_trajectory_point.yaw)], [np.sin(des_trajectory_point.yaw)], [0]]) y_body_in_world = np.cross(Rot_des[:3, 2], x_body_in_world, axis=0) Rot_des[:3, 1] = np.matrix(y_body_in_world / LA.norm(y_body_in_world)) Rot_des[:3, 0] = np.matrix(np.cross(Rot_des[:3, 1], Rot_des[:3, 2], axis=0)) quad_des = tf.transformations.quaternion_from_matrix(Rot_des) attitude_des.orientation.x = quad_des[0] attitude_des.orientation.y = quad_des[1] attitude_des.orientation.z = quad_des[2] attitude_des.orientation.w = quad_des[3] attitude_des.thrust = thrust.thrust self.mavros_attitude_pub.publish(attitude_des) self.mavros_thrust_pub.publish(thrust) self.time_prev = time_now
def lift_off_rec(self, thrust, time_flying): recursions = self.calculate_recursions(time_flying) print(recursions) self.beh_type = "TAKE OFF" for i in range(recursions): print(i) if self.beh_type == "TAKE OFF" and thrust < self.Liftoff_thrust: #and beh_type == "HOVER": print("Lifting the drone up slowly") target_raw_attitude = AttitudeTarget() target_raw_attitude.header.stamp = rospy.Time.now() target_raw_attitude.orientation = self.imu.orientation target_raw_attitude.body_rate.x = 0 # ROLL_RATE target_raw_attitude.body_rate.y = 0 # PITCH_RATE target_raw_attitude.body_rate.z = 0 # YAW_RATE target_raw_attitude.thrust = thrust thrust = thrust + self.accumulating_thrust self.attitude_target_pub.publish(target_raw_attitude) time.sleep(self.Time_between_messages ) # was 0.005 (now 50hz ,500 loops ,5sec) elif self.beh_type == "TAKE OFF" and thrust >= self.Liftoff_thrust and self.down_sensor_distance <= self.hover_sensor_altitude_min: print("The drone is lifting off at constant thrust") target_raw_attitude = AttitudeTarget() target_raw_attitude.header.stamp = rospy.Time.now() target_raw_attitude.orientation = self.imu.orientation target_raw_attitude.body_rate.x = 0 # ROLL_RATE target_raw_attitude.body_rate.y = 0 # PITCH_RATE target_raw_attitude.body_rate.z = 0 # YAW_RATE thrust = self.Liftoff_thrust target_raw_attitude.thrust = thrust self.attitude_target_pub.publish(target_raw_attitude) time.sleep(self.Time_between_messages ) #was 0.005 (now 50hz ,500loops) else: break self.beh_type = 'HOVER' print("time of lifting off has ended") self.hover_rec(self.hover_time)
def send_commands(self, event): command_msg = AttitudeTarget() command_msg.header.stamp = rospy.get_rostime() command_msg.type_mask = self.ignore_mask command_msg.body_rate.x = 0.0 command_msg.body_rate.y = 0.0 command_msg.body_rate.z = 0.0 command_msg.thrust = self.thrust_val print self.thrust_val print "here" # Publish. self.land_pub.publish(command_msg)
def publishControlInputs(self): hcc = AttitudeControlExt() hcc.header.stamp = rospy.Time.now() hcc.thrust = self.desiredThrust #gain = 100.0 *0.5 hcc.roll = self.taus[0] hcc.pitch = -self.taus[1] hcc.yaw = -self.taus[2] ##################################################### target = AttitudeTarget() target.header.stamp=rospy.Time.now() target.body_rate.x = self.taus[0] target.body_rate.y = -self.taus[1] target.body_rate.z = -self.taus[2] target.thrust = self.desiredThrust ######################################################### output = HippocampusOutput_2() output.frame_stamp = rospy.Time.now() output.des_rates.x = self.desired_rates[0] output.des_rates.y = self.desired_rates[1] output.des_rates.z = self.desired_rates[2] output.current_rates.x = self.body_rate[0] output.current_rates.y = self.body_rate[1] output.current_rates.z = self.body_rate[2] outputx = HippocampusOutput() outputx.frame_stamp = rospy.Time.now() outputx.current_orientation.x = self.current_axis[0] outputx.current_orientation.y = self.current_axis[1] outputx.current_orientation.z = self.current_axis[2] outputx.des_orientation.x = self.desired_axis[0] outputx.des_orientation.y = self.desired_axis[1] outputx.des_orientation.z = self.desired_axis[2] outputx.current_velocity.x = self.desired_rates[0] outputx.current_velocity.y = self.desired_rates[1] outputx.current_velocity.z = self.desired_rates[2] # self.output_pub.publish(outputx) # self.output_2_pub.publish(output) # self.control_pub.publish(hcc) self.control_pub3.publish(target)
def body_recursion(self, thrust): if self.status_thrust == "up": if (thrust <= -1): return True else: target_raw_attitude = AttitudeTarget() target_raw_attitude.header.stamp = rospy.Time.now() target_raw_attitude.type_mask = AttitudeTarget.IGNORE_ATTITUDE target_raw_attitude.thrust = 0.5 target_raw_attitude.body_rate.x = 0 # ROLL_RATE target_raw_attitude.body_rate.y = thrust - 0.003 # PITCH_RATE target_raw_attitude.body_rate.z = 0 # YAW_RATE self.attitude_target_pub.publish(target_raw_attitude) time.sleep(0.1) return self.body_recursion(target_raw_attitude.body_rate.y)
def secure_landing_phase_rec(self, thrust, time_flying): if time_flying <= 0: return True else: target_raw_attitude = AttitudeTarget() target_raw_attitude.header.stamp = rospy.Time.now() target_raw_attitude.orientation = self.imu.orientation target_raw_attitude.body_rate.x = 0 # ROLL_RATE target_raw_attitude.body_rate.y = 0 # PITCH_RATE target_raw_attitude.body_rate.z = 0 # YAW_RATE target_raw_attitude.thrust = self.Landing_thrust thrust = self.Landing_thrust self.attitude_target_pub.publish(target_raw_attitude) time_flying = time_flying - self.Time_between_messages time.sleep(self.Time_between_messages ) # was 0.005 (now 50hz ,500 loops ,5sec) return self.secure_landing_phase_rec( thrust, beh_type, time_flying ) #bublishing a constant parameter "not updating thrust argument"