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 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 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 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 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 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 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 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 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 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 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 setUp(self): self.local_position = PoseStamped() self.state = State() self.att = AttitudeTarget() self.sub_topics_ready = { key: False for key in ['local_pos', 'home_pos', 'state'] } # setup ROS topics and services try: rospy.wait_for_service('mavros/cmd/arming', 30) rospy.wait_for_service('mavros/set_mode', 30) except rospy.ROSException: self.fail("failed to connect to mavros services") self.set_arming_srv = rospy.ServiceProxy('mavros/cmd/arming', CommandBool) self.set_mode_srv = rospy.ServiceProxy('mavros/set_mode', SetMode) self.local_pos_sub = rospy.Subscriber('mavros/local_position/pose', PoseStamped, self.local_position_callback) self.home_pos_sub = rospy.Subscriber('mavros/home_position/home', HomePosition, self.home_position_callback) self.state_sub = rospy.Subscriber('mavros/state', State, self.state_callback) self.att_setpoint_pub = rospy.Publisher('mavros/setpoint_raw/attitude', AttitudeTarget, queue_size=10) # send setpoints in seperate thread to better prevent failsafe self.att_thread = Thread(target=self.send_att, args=()) self.att_thread.daemon = True self.att_thread.start()
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 _send_command(self, data): #rospy.loginfo(data) self.local_pose = data # debug quaternion = (data.pose.orientation.x, data.pose.orientation.y, data.pose.orientation.z, data.pose.orientation.w) euler = tf.transformations.euler_from_quaternion(quaternion) #print 'roll is %5.2f deg, and pitch is %5.2f deg, and yaw is %5.2f deg' % (euler[0]*180/3.1425, euler[1]*180/3.1425, euler[2]*180/3.1425) [self.cpitch,self.croll,self.cyaw,self.cheight] = \ self.controller.genQUADcontrol(self.local_pose.pose,self.waypoint.pose,self.velocity) #print 'y command %5.2f, x command %5.2f, yaw command %5.2f, thrust %5.2f' % (self.croll, self.cpitch, self.cyaw, self.cheight + self.hoverth) self.AttitudeTarget = AttitudeTarget() self.AttitudeTarget.orientation = Quaternion( *quaternion_from_euler(self.croll, self.cpitch, self.cyaw)) self.AttitudeTarget.thrust = self.cheight + self.hoverth if inside_boundary(self.local_pose.pose, self.boundary): result_mode = self.change_mode(0, "OFFBOARD") self.pub_sp.publish(self.AttitudeTarget) else: result_mode = self.change_mode(0, "POSCTL")
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"
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 __init__(self): self.rate = rospy.Rate(4) self.connected = False self.took_off = False self.init_control = False self.m = 30 self.c12 = 1 self.c13 = 1 self.c14 = 1 self.c15 = 1 self.c16 = 0.0 self.c17 = 0.0 # self.c16 = 0.015 # self.c17 = 0.015 self.lam6 = 0.015 self.lam7 = 0.015 self.ffactor = 1 self.kp = 50 self.agents = {} self.network = two_agents self.agent_names = [ rospy.get_param(s) for s in rospy.get_param_names() if "mambo_request/name" in s ] rospy.loginfo(self.agent_names) for s in self.agent_names: ## Dictionary of each agent's state information. The two zeros are the integration of velocity errors. self.agents[s] = [ PoseStamped(), TwistStamped(), TwistStamped(), AttitudeTarget(), (0.0, 0.0, 0.0), 0.0, 0.0 ] ## Publishers and subscribers for each agent. self.pub_disconnect = rospy.Publisher('/mambo_srv/' + s + '/disconnect', String, queue_size=10) self.pub_fly_command = rospy.Publisher('/mambo_srv/' + s + '/fly_command', AttitudeTarget, queue_size=10) self.sub_pose = rospy.Subscriber( '/vrpn_client_node/' + s + '/pose', PoseStamped, self.callback_pose, s) self.sub_twist = rospy.Subscriber( '/vrpn_client_node/' + s + '/twist', TwistStamped, self.callback_twist, s) self.sub_accel = rospy.Subscriber( '/vrpn_client_node/' + s + '/accel', TwistStamped, self.callback_accel, s)
def mocapcb(pose, twist): ## Initialise Attitude Target message, the control input to be sent to host_service.py u = AttitudeTarget() # ## Get orientation from mocap data and convert to Euler angles # orientation_q = pose.pose.orientation # orientation_list = [orientation_q.x, orientation_q.y, orientation_q.z, orientation_q.w] # (roll, pitch, yaw) = euler_from_quaternion(orientation_list) # rospy.loginfo('%f' % roll + ' and %f' % pitch) ## Calculate the velocity command from integral backstepping: https://doi.org/10.3929/ethz-a-010039365 chi6 = 0 ## IMPLEMENT INTEGRATION chi7 = 0 ## IMPLEMENT INTEGRATION e10 = xref - twist.twist.linear.x e11 = yref - twist.twist.linear.y u.thrust = zref u.orientation.x = (alt_gains.m / u.thrust) * (( (alt_gains.c12 + alt_gains.c13) * e10) + (alt_gains.lam6 * chi6)) u.orientation.y = (alt_gains.m / u.thrust) * (( (alt_gains.c14 + alt_gains.c15) * e11) + (alt_gains.lam7 * chi7)) rospy.loginfo('Calculated z %f, phi %f and theta %f', u.thrust, u.orientation.x, u.orientation.y)
def setUp(self): super(MavrosOffboardAttctlTest, self).setUp() self.att = AttitudeTarget() self.att_setpoint_pub = rospy.Publisher( 'mavros/setpoint_raw/attitude', AttitudeTarget, queue_size=1) # send setpoints in seperate thread to better prevent failsafe self.att_thread = Thread(target=self.send_att, args=()) self.att_thread.daemon = True self.att_thread.start()
def __init__(self): self.rate = rospy.Rate(4) self.connected = False self.took_off = False self.init_control = False self.m = 30 self.c12 = 1 self.c13 = 1 self.c14 = 1 self.c15 = 1 self.c16 = 0.015 self.c17 = 0.015 self.lam6 = 0.015 self.lam7 = 0.015 self.chi6 = 0 self.chi7 = 0 self.ffactor = 1 self.kp = 50 self.pose = PoseStamped() self.twist = TwistStamped() self.accel = TwistStamped() self.u = AttitudeTarget() self.xdref = 0 self.ydref = 0 self.zdref = 0 self.agent_names = [ rospy.get_param(s) for s in rospy.get_param_names() if "mambo_request/name" in s ] rospy.loginfo(self.agent_names) for s in self.agent_names: ## Publishers and subscribers for each agent. self.pub_disconnect = rospy.Publisher('/mambo_srv/' + s + '/disconnect', String, queue_size=10) self.pub_fly_command = rospy.Publisher('/mambo_srv/' + s + '/fly_command', AttitudeTarget, queue_size=10) self.sub_pose = rospy.Subscriber( '/vrpn_client_node/' + s + '/pose', PoseStamped, self.callback_pose) self.sub_twist = rospy.Subscriber( '/vrpn_client_node/' + s + '/twist', TwistStamped, self.callback_twist) self.sub_accel = rospy.Subscriber( '/vrpn_client_node/' + s + '/accel', TwistStamped, self.callback_accel)
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 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 __init__(self, id=""): self.current_state = State() self.uav_id = id self.targetattitude = AttitudeTarget() self.targetattitude.orientation.x = 0 self.targetattitude.orientation.y = 0 self.targetattitude.orientation.z = 0 self.targetattitude.orientation.w = 1 self.targetattitude.thrust = 0.1 self.position = PoseStamped() self.velocity = TwistStamped()
def __init__(self): rospy.init_node('pad', anonymous=True) self.threshold = .1 #because the controler is a little sticky #self.offset=rospy.get_param('~offset') self.axes_map = { 'roll': 3, 'pitch': 4, 'yaw': 0, 'throttle': 1, 'mode_set1': 6, 'mode_set2': 7 } self.velocity_scale = {'angular': 1.5, 'linear': 2.0} self.button_map = { 'arm': 0, #a 'disarm': 1, #b 'takeoff': 2, #x 'land': 3, #y 'enable': 4 #rb } self.mode = "POS" # three modes: POS, VEL, ATD self.joy_sub = rospy.Subscriber("joy", Joy, self.joy_cb) #state info self.pose = Pose() self.agentPose_sub = rospy.Subscriber('mavros/local_position/pose', PoseStamped, self.poseCB) #pose control self.cmdPose = Pose() self.cmdPose.orientation.w = -1 self.cmdPose_pub = rospy.Publisher('mavros/setpoint_position/local', PoseStamped, queue_size=10) #atd control self.cmdAtd = AttitudeTarget() self.cmdAtd_pub = rospy.Publisher('mavros/setpoint_raw/attitude', AttitudeTarget, queue_size=10) #velocity control self.cmdVel = Twist() self.cmdVel_pub = self.agentVel_cmd = rospy.Publisher( 'mavros/setpoint_velocity/cmd_vel_unstamped', Twist, queue_size=10)