def send_commands(self, event): # Create the NED velocity vector. ned_vec = np.array([[self.vel_x], [self.vel_y], [self.vel_z]], dtype=np.float32) # Rotate the NED vector to ENU (for MAVROS). enu_vec = self.ned_to_enu(ned_vec) # Create the PositionTarget command message. command_msg = PositionTarget() # Fill out the message. command_msg.header.stamp = rospy.get_rostime() # FRAME_BODY_NED is the vehicle-1 frame command_msg.coordinate_frame = command_msg.FRAME_BODY_NED command_msg.type_mask = self.ignore_mask command_msg.velocity.x = enu_vec[0][0] command_msg.velocity.y = enu_vec[1][0] command_msg.velocity.z = enu_vec[2][0] command_msg.yaw_rate = -self.yaw_rate # Publish the message. self.command_pub.publish(command_msg)
def construct_target(self, x, y, z, yaw, yaw_rate=1): """ This is a function that will constuct a valid target message for the drone :param x: Defining the target function in terms of x coordinates(Target) :param y: Defining the target function in terms of y coordinates(Target) :param z: Defining the target function in terms of z coordinates(Target) :param yaw: yaw movement to move towards target :param yaw_rate: Yaw rate is 1 ms :rtype: target_raw_pose :return: Target Position """ target_raw_pose = PositionTarget() target_raw_pose.header.stamp = rospy.Time.now() target_raw_pose.coordinate_frame = 9 target_raw_pose.position.x = x target_raw_pose.position.y = y target_raw_pose.position.z = z target_raw_pose.type_mask = PositionTarget.IGNORE_VX + PositionTarget.IGNORE_VY + PositionTarget.IGNORE_VZ \ + PositionTarget.IGNORE_AFX + PositionTarget.IGNORE_AFY + PositionTarget.IGNORE_AFZ \ + PositionTarget.FORCE target_raw_pose.yaw = yaw target_raw_pose.yaw_rate = yaw_rate return target_raw_pose
def send_waypoint_command(self): if self.mode_flag == 'mavros': waypoint_command_msg = PositionTarget() waypoint_command_msg.header.stamp = rospy.get_rostime() waypoint_command_msg.coordinate_frame = waypoint_command_msg.FRAME_LOCAL_NED waypoint_command_msg.type_mask = self.ned_pos_mask waypoint_command_msg.position.x = self.wp_E # E waypoint_command_msg.position.y = self.wp_N # N waypoint_command_msg.position.z = self.rendezvous_height # U # This converts an NED heading command into an ENU heading command yaw = -self.heading_command + np.radians(90.0) while yaw > np.radians(180.0): yaw = yaw - np.radians(360.0) while yaw < np.radians(-180.0): yaw = yaw + np.radians(360.0) waypoint_command_msg.yaw = yaw # Publish. self.command_pub_mavros.publish(waypoint_command_msg) else: wp_command_msg = Command() wp_command_msg.x = self.wp_N wp_command_msg.y = self.wp_E wp_command_msg.F = -self.rendezvous_height wp_command_msg.z = self.heading_command wp_command_msg.mode = Command.MODE_XPOS_YPOS_YAW_ALTITUDE # Publish. self.command_pub_roscopter.publish(wp_command_msg)
def cmdvel2PosTarget(vel): ''' Translates from JderobotTypes CMDVel to ROS Twist. @param vel: JderobotTypes CMDVel to translate @type img: JdeRobotTypes.CMDVel @return a Twist translated from vel ''' msg = PositionTarget(header=Header(stamp=rospy.Time.now(), frame_id=''), ) msg.coordinate_frame = 8 msg.type_mask = 1475 msg.position.x = vel.px msg.position.y = vel.py msg.position.z = vel.pz msg.velocity.x = vel.vx msg.velocity.y = vel.vy msg.velocity.z = vel.vz msg.acceleration_or_force.x = vel.ax msg.acceleration_or_force.y = vel.ay msg.acceleration_or_force.z = vel.az msg.yaw = vel.yaw msg.yaw_rate = vel.yaw_rate return msg
def construct_target(self, x=0, y=0, z=0, vx=0, vy=0, vz=0, yaw=0): target_raw_pose = PositionTarget() target_raw_pose.coordinate_frame = self.coordinate_frame if self.coordinate_frame == 1: target_raw_pose.position.x = x target_raw_pose.position.y = y target_raw_pose.position.z = z target_raw_pose.velocity.x = vx target_raw_pose.velocity.y = vy target_raw_pose.velocity.z = vz else: target_raw_pose.position.x = -y target_raw_pose.position.y = x target_raw_pose.position.z = z target_raw_pose.velocity.x = -vy target_raw_pose.velocity.y = vx target_raw_pose.velocity.z = vz target_raw_pose.yaw = yaw if (self.motion_type == 0): target_raw_pose.type_mask = PositionTarget.IGNORE_VX + PositionTarget.IGNORE_VY + PositionTarget.IGNORE_VZ \ + PositionTarget.IGNORE_AFX + PositionTarget.IGNORE_AFY + PositionTarget.IGNORE_AFZ + PositionTarget.IGNORE_YAW_RATE if (self.motion_type == 1): target_raw_pose.type_mask = PositionTarget.IGNORE_PX + PositionTarget.IGNORE_PY + PositionTarget.IGNORE_PZ \ + PositionTarget.IGNORE_AFX + PositionTarget.IGNORE_AFY + PositionTarget.IGNORE_AFZ \ + PositionTarget.IGNORE_YAW_RATE return target_raw_pose
def set_vel_body_yaw_rate(self, vel_body, yaw_rate, freq): """ Offboard method that sends velocity_body and yaw_rate references to the PX4 autopilot of the the drone. Makes convertions from the body NED frame adopted for the ISR Flying Arena to the body ENU frame used by ROS. Parameters ---------- vel_body : np.array of floats with shape (3,1) Desired linear velocity for the drone, in body NED coordinates. yaw_rate : float Desired yaw rate for the vehicle, in radians per second. freq : float Topic publishing frequency, in Hz. """ pub = rospy.Publisher(self.info.drone_ns + '/mavros/setpoint_raw/local', PositionTarget, queue_size=1) msg = PositionTarget() msg.header = Header() msg.header.stamp = rospy.Time.now() msg.coordinate_frame = 8 msg.type_mask = 1991 msg.velocity.x, msg.velocity.y, msg.velocity.z = ned_to_enu(vel_body) msg.yaw_rate = -yaw_rate pub.publish(msg) rate = rospy.Rate(freq) rate.sleep()
def construct_target(self, x, y, z, yaw, x_vel=0.2, y_vel=0.2, z_vel=0.2, yaw_rate=1): target_raw_pose = PositionTarget( ) # We will fill the following message with our values: http://docs.ros.org/api/mavros_msgs/html/msg/PositionTarget.html target_raw_pose.header.stamp = rospy.Time.now() target_raw_pose.coordinate_frame = 9 target_raw_pose.position.x = x target_raw_pose.position.y = y target_raw_pose.position.z = z target_raw_pose.velocity.x = x_vel target_raw_pose.velocity.y = y_vel target_raw_pose.velocity.z = z_vel target_raw_pose.type_mask = PositionTarget.IGNORE_AFX + PositionTarget.IGNORE_AFY + PositionTarget.IGNORE_AFZ \ + PositionTarget.FORCE target_raw_pose.yaw = yaw target_raw_pose.yaw_rate = yaw_rate return target_raw_pose
def forward(self): msg = TwistStamped() msg.header.stamp = rospy.Time.now() msg.twist.angular.z = 0 print("forward") vel = PositionTarget() vel.header.stamp = rospy.Time.now() vel.coordinate_frame = PositionTarget.FRAME_BODY_NED vel.type_mask = PositionTarget.IGNORE_PX + PositionTarget.IGNORE_PY + PositionTarget.IGNORE_PZ + PositionTarget.IGNORE_AFY + PositionTarget.IGNORE_AFZ + PositionTarget.FORCE + PositionTarget.IGNORE_YAW + PositionTarget.IGNORE_YAW_RATE vel.velocity.x = 0 vel.velocity.y = 1.0 vel.velocity.z = 0 bt_missions.p0 = (bt_missions.current_.latitude, bt_missions.current_.longitude) bt_missions.p1 = (bt_missions.target[bt_missions.index][0], bt_missions.target[bt_missions.index][1]) bt_missions.dist = vincenty(bt_missions.p0, bt_missions.p1).meters forward_t = rospy.get_time() while rospy.get_time( ) - forward_t <= bt_missions.dist * 0.1: #bt_missions.dist *1.25*0.25: #rospy.wait_for_service('/mavros/set_mode') change_mode = rospy.ServiceProxy('/mavros/set_mode', SetMode) response = change_mode(custom_mode="GUIDED") bt_missions.set_vel_pub.publish(msg) bt_missions.set_v_pub.publish(vel) bt_missions.rate.sleep()
def construct_target(self, x=0, y=0, z=0, vx=0, vy=0, vz=0, afx=0, afy=0, afz=0, yaw=0, yaw_rate=0): target_raw_pose = PositionTarget() target_raw_pose.coordinate_frame = self.coordinate_frame target_raw_pose.position.x = x target_raw_pose.position.y = y target_raw_pose.position.z = z target_raw_pose.velocity.x = vx target_raw_pose.velocity.y = vy target_raw_pose.velocity.z = vz target_raw_pose.acceleration_or_force.x = afx target_raw_pose.acceleration_or_force.y = afy target_raw_pose.acceleration_or_force.z = afz target_raw_pose.yaw = yaw target_raw_pose.yaw_rate = yaw_rate if(self.motion_type == 0): target_raw_pose.type_mask = PositionTarget.IGNORE_VX + PositionTarget.IGNORE_VY + PositionTarget.IGNORE_VZ \ + PositionTarget.IGNORE_AFX + PositionTarget.IGNORE_AFY + PositionTarget.IGNORE_AFZ \ + PositionTarget.IGNORE_YAW_RATE if(self.motion_type == 1): target_raw_pose.type_mask = PositionTarget.IGNORE_PX + PositionTarget.IGNORE_PY + PositionTarget.IGNORE_PZ \ + PositionTarget.IGNORE_AFX + PositionTarget.IGNORE_AFY + PositionTarget.IGNORE_AFZ \ + PositionTarget.IGNORE_YAW if(self.motion_type == 2): target_raw_pose.type_mask = PositionTarget.IGNORE_PX + PositionTarget.IGNORE_PY + PositionTarget.IGNORE_PZ \ + PositionTarget.IGNORE_VX + PositionTarget.IGNORE_VY + PositionTarget.IGNORE_VZ \ + PositionTarget.IGNORE_YAW return target_raw_pose
def offboard_loop(mode): pub_pt = {} #создаем топики, для публикации управляющих значений for n in range(1, instances_num + 1): pub_pt[n] = rospy.Publisher(f"/mavros{n}/setpoint_raw/local", PositionTarget, queue_size=10) pt = PositionTarget() #см. также описание mavlink сообщения https://mavlink.io/en/messages/common.html#SET_POSITION_TARGET_LOCAL_NED pt.coordinate_frame = pt.FRAME_LOCAL_NED t0 = time.time() #цикл управления rate = rospy.Rate(freq) while not rospy.is_shutdown(): dt = time.time() - t0 #управляем каждым аппаратом централизованно for n in range(1, instances_num + 1): set_mode(n, "OFFBOARD") if mode == 0: mc_example(pt, n, dt) elif mode == 1: vtol_example(pt, n, dt) pub_pt[n].publish(pt) rate.sleep()
def altitude_target(altitude, yaw=0.0, frame_id='world'): pt = PositionTarget() pt.header.stamp = rospy.Time.now() pt.header.frame_id = frame_id pt.coordinate_frame = 1 pt.type_mask = POSITION_MASK pt.position.z = altitude pt.yaw = yaw # in radians! return pt
def Intarget_local(): set_target_local = PositionTarget() set_target_local.type_mask = 0b100111111000 set_target_local.coordinate_frame = 1 set_target_local.position.x = 10 set_target_local.position.y = 10 set_target_local.position.z = 10 set_target_local.yaw = 0 return set_target_local
def twist_obj(x, y, z, a, b, c): move_cmd = PositionTarget(header=Header(stamp=rospy.get_rostime())) move_cmd.velocity.x = x move_cmd.velocity.y = y move_cmd.velocity.z = z move_cmd.yaw_rate = c move_cmd.coordinate_frame = 8 move_cmd.type_mask = 1479 # 1479/ignore_all_except_v_xyz_yr = (1 << 10) | (7 << 6) | (7 << 0) return move_cmd
def send_ned_velocity(v_x, v_y, v_z): set_velocity = PositionTarget() set_velocity.coordinate_frame = 1 # local NED set_velocity.velocity.x = v_y # verificare che le componenti siano invertite set_velocity.velocity.y = v_x set_velocity.velocity.z = v_z set_velocity.type_mask = 4039 setpoint_velocity_pub.publish(set_velocity) time.sleep(0.5)
def set_velocity_body(v_x, v_y, v_z, rate): set_velocity = PositionTarget() set_velocity.coordinate_frame = 8 # body NED set_velocity.velocity.x = v_y # verificare che le componenti siano invertite set_velocity.velocity.y = v_x set_velocity.velocity.z = v_z set_velocity.yaw_rate = rate set_velocity.type_mask = 1479 setpoint_velocity_pub.publish(set_velocity)
def construct_target(self, x=0, y=0, z=0, vx=0, vy=0, vz=0, afx=0, afy=0, afz=0, yaw=0, yaw_rate=0): target_raw_pose = PositionTarget() target_raw_pose.coordinate_frame = self.coordinate_frame if self.coordinate_frame == 1: target_raw_pose.position.x = x target_raw_pose.position.y = y target_raw_pose.position.z = z else: target_raw_pose.position.x = -y target_raw_pose.position.y = x target_raw_pose.position.z = z if self.transition_state == 'plane': if self.plane_mission == 'takeoff': target_raw_pose.type_mask = 4096 elif self.plane_mission == 'land': target_raw_pose.type_mask = 8192 elif self.plane_mission == 'loiter': target_raw_pose.type_mask = 12288 else: target_raw_pose.type_mask = 16384 else: if self.coordinate_frame == 1: target_raw_pose.velocity.x = vx target_raw_pose.velocity.y = vy target_raw_pose.velocity.z = vz target_raw_pose.acceleration_or_force.x = afx target_raw_pose.acceleration_or_force.y = afy target_raw_pose.acceleration_or_force.z = afz else: target_raw_pose.velocity.x = -vy target_raw_pose.velocity.y = vx target_raw_pose.velocity.z = vz target_raw_pose.acceleration_or_force.x = -afy target_raw_pose.acceleration_or_force.y = afx target_raw_pose.acceleration_or_force.z = afz target_raw_pose.yaw = yaw target_raw_pose.yaw_rate = yaw_rate if(self.motion_type == 0): target_raw_pose.type_mask = PositionTarget.IGNORE_VX + PositionTarget.IGNORE_VY + PositionTarget.IGNORE_VZ \ + PositionTarget.IGNORE_AFX + PositionTarget.IGNORE_AFY + PositionTarget.IGNORE_AFZ \ + PositionTarget.IGNORE_YAW_RATE if(self.motion_type == 1): target_raw_pose.type_mask = PositionTarget.IGNORE_PX + PositionTarget.IGNORE_PY + PositionTarget.IGNORE_PZ \ + PositionTarget.IGNORE_AFX + PositionTarget.IGNORE_AFY + PositionTarget.IGNORE_AFZ \ + PositionTarget.IGNORE_YAW if(self.motion_type == 2): target_raw_pose.type_mask = PositionTarget.IGNORE_PX + PositionTarget.IGNORE_PY + PositionTarget.IGNORE_PZ \ + PositionTarget.IGNORE_VX + PositionTarget.IGNORE_VY + PositionTarget.IGNORE_VZ \ + PositionTarget.IGNORE_YAW return target_raw_pose
def takeoff_obj(z): print("--------------------Takeoff-----------------") move_cmd = PositionTarget(header=Header(stamp=rospy.get_rostime())) move_cmd.velocity.x = 0 move_cmd.velocity.y = 0 move_cmd.velocity.z = 0.5 move_cmd.position.z = z move_cmd.type_mask = (0x1000 | 0b110111000011) move_cmd.coordinate_frame = 9 return move_cmd
def custom(twisted): #print("custom1") move_cmd = PositionTarget(header=Header(stamp=rospy.get_rostime())) move_cmd.velocity.x = twisted.linear.y move_cmd.velocity.y = twisted.linear.x move_cmd.velocity.z = twisted.linear.z move_cmd.yaw_rate = twisted.angular.z move_cmd.coordinate_frame = 8 move_cmd.type_mask = 1479 # 1479/ignore_all_except_v_xyz_yr = (1 << 10) | (7 << 6) | (7 << 0) pub.publish(move_cmd)
def get_descent(self,x,y,z): des_vel = PositionTarget() des_vel.header.frame_id = "world" des_vel.header.stamp=rospy.Time.from_sec(time.time()) des_vel.coordinate_frame= 8 des_vel.type_mask = 3527 des_vel.velocity.x = x des_vel.velocity.y = y des_vel.velocity.z = z return des_vel
def displacement(x_disp, y_disp, z_disp): print("I am in displacement") des_disp = PositionTarget() des_disp.header.frame_id = "world" des_disp.header.stamp = rospy.Time.from_sec(time.time()) des_disp.coordinate_frame = 8 des_disp.type_mask = 3527 des_disp.velocity.x = x_disp des_disp.velocity.y = y_disp des_disp.velocity.z = z_disp return des_disp
def position_target(position, yaw=0.0, frame_id='world'): pt = PositionTarget() pt.header.stamp = rospy.Time.now() pt.header.frame_id = frame_id pt.coordinate_frame = 1 pt.type_mask = POSITION_MASK pt.position.x = position[0] pt.position.y = position[1] pt.position.z = position[2] pt.yaw = yaw # in radians! return pt
def construct_target(self, x, y, z, yaw, yaw_rate = 1): target_raw_pose = PositionTarget() target_raw_pose.header.stamp = rospy.Time.now() target_raw_pose.coordinate_frame = 9 target_raw_pose.position.x = x target_raw_pose.position.y = y target_raw_pose.position.z = z target_raw_pose.type_mask = PositionTarget.IGNORE_VX + PositionTarget.IGNORE_VY + PositionTarget.IGNORE_VZ + PositionTarget.IGNORE_AFX + PositionTarget.IGNORE_AFY + PositionTarget.IGNORE_AFZ + PositionTarget.FORCE target_raw_pose.yaw = yaw target_raw_pose.yaw_rate = yaw_rate return target_raw_pose
def velocity_target(velocity, frame_id='world'): pt = PositionTarget() pt.header.stamp = rospy.Time.now() pt.header.frame_id = frame_id pt.coordinate_frame = 1 pt.type_mask = VELOCITY_MASK pt.velocity.x = velocity[0] pt.velocity.y = velocity[1] pt.velocity.z = velocity[2] pt.yaw = position_to_angle(velocity[0], velocity[1]) return pt
def twist_obj(x, y, z, a, b, c): # move_cmd = Twist() move_cmd = PositionTarget() move_cmd.velocity.x = x move_cmd.velocity.y = y move_cmd.velocity.z = z # move_cmd.yaw_rate = c move_cmd.header.stamp = rospy.get_rostime() move_cmd.header.frame_id = "world" move_cmd.coordinate_frame = 8 move_cmd.type_mask = 3527 return move_cmd
def global_position(): global home_ global home_set_ global current_ global brng global dirct, scale global current_heading global current_yaw guide() hdg_subscriber = rospy.Subscriber('/mavros/global_position/compass_hdg', Float64, compass) home_sub = rospy.Subscriber("/mavros/home_position/home", HomePosition, setHomeGeoPointCB) set_vel_pub = rospy.Publisher('/mavros/setpoint_velocity/cmd_vel', TwistStamped, queue_size=10) set_v_pub = rospy.Publisher('/mavros/setpoint_yaw/local', PositionTarget, queue_size=10) gps_sub = rospy.Subscriber("/mavros/global_position/global", NavSatFix, current_position) rospy.Subscriber("/mavros/global_position/raw/fix", NavSatFix, gpsstate_cb) gps_pub = rospy.Publisher('/mavros/global_position/raw/gps_vel', TwistStamped, queue_size=10) #altitude_sub = rospy.Subscriber('/mavros/altitude', Altitude, alt_cb) local_pos_pub = rospy.Publisher(mavros.get_topic('setpoint_position', 'local'), PoseStamped, queue_size=10) time_now = rospy.get_time() while rospy.get_time() - time_now <= 3: pass print("gps now: %s" % gps_state_.status) if gps_state_.status == -1: RTL() else: msg = TwistStamped() msg.header.stamp = rospy.Time.now() msg.twist.angular.z = 0 print("forward") vel = PositionTarget() vel.header.stamp = rospy.Time.now() vel.coordinate_frame = PositionTarget.FRAME_BODY_NED vel.type_mask = PositionTarget.IGNORE_PX + PositionTarget.IGNORE_PY + PositionTarget.IGNORE_PZ + PositionTarget.IGNORE_AFY + PositionTarget.IGNORE_AFZ + PositionTarget.FORCE + PositionTarget.IGNORE_YAW + PositionTarget.IGNORE_YAW_RATE vel.velocity.x = 0 vel.velocity.y = 1 vel.velocity.z = 0 forward_t = rospy.get_time() while rospy.get_time() - forward_t <= 5: set_vel_pub.publish(msg) set_v_pub.publish(vel) rate.sleep()
def getvelBody(self, u, v, w): msg = PositionTarget() msg.header.stamp = rospy.Time.now() msg.coordinate_frame = 8 msg.type_mask = 4039 msg.velocity.x = u msg.velocity.y = v msg.velocity.z = w r = rospy.Rate(10) # 10hz while not rospy.is_shutdown(): self.pub2.publish(msg) r.sleep()
def circle(self, x, y, z): msg = PositionTarget() msg.header.stamp = rospy.Time.now() msg.coordinate_frame = 8 msg.type_mask = 1543 msg.velocity.x = 0 msg.velocity.y = -1 msg.velocity.z = 0 msg.acceleration_or_force.x = -0.2 msg.acceleration_or_force.y = 0 msg.acceleration_or_force.z = 0 msg.yaw_rate = -0.2 self.pub2.publish(msg)
def forward(self): print(123) forward_t = rospy.get_time() #while rospy.get_time() - forward_t <= 5: bt_missions.dis_count = False vel = PositionTarget() vel.header.stamp = rospy.Time.now() vel.coordinate_frame = PositionTarget.FRAME_BODY_NED vel.type_mask = PositionTarget.IGNORE_PX + PositionTarget.IGNORE_PY + PositionTarget.IGNORE_PZ + PositionTarget.IGNORE_AFY + PositionTarget.IGNORE_AFZ + PositionTarget.FORCE + PositionTarget.IGNORE_YAW + PositionTarget.IGNORE_YAW_RATE vel.velocity.x = 0 vel.velocity.y = 0.8 vel.velocity.z = 0 bt_missions.set_v_pub.publish(vel) bt_missions.rate.sleep()
def construct_target(self, x=0, y=0, z=0, vx=0, vy=0, vz=0, afx=0, afy=0, afz=0, yaw=0, yaw_rate=0): target_raw_pose = PositionTarget() target_raw_pose.coordinate_frame = self.coordinate_frame target_raw_pose.position.x = x target_raw_pose.position.y = y target_raw_pose.position.z = z target_raw_pose.velocity.x = vx target_raw_pose.velocity.y = vy target_raw_pose.velocity.z = vz target_raw_pose.acceleration_or_force.x = afx target_raw_pose.acceleration_or_force.y = afy target_raw_pose.acceleration_or_force.z = afz if self.is_mavros_yaw_different: # rotate 90 degrees target_raw_pose.yaw = (yaw + 3 * math.pi / 2) if ( yaw - math.pi / 2 < -math.pi) else (yaw - math.pi / 2) target_raw_pose.position.x = y target_raw_pose.position.y = -x else: target_raw_pose.yaw = yaw target_raw_pose.yaw_rate = yaw_rate if (self.motion_type == 0): target_raw_pose.type_mask = PositionTarget.IGNORE_VX + PositionTarget.IGNORE_VY + PositionTarget.IGNORE_VZ \ + PositionTarget.IGNORE_AFX + PositionTarget.IGNORE_AFY + PositionTarget.IGNORE_AFZ \ + PositionTarget.IGNORE_YAW_RATE if (self.motion_type == 1): target_raw_pose.type_mask = PositionTarget.IGNORE_PX + PositionTarget.IGNORE_PY + PositionTarget.IGNORE_PZ \ + PositionTarget.IGNORE_AFX + PositionTarget.IGNORE_AFY + PositionTarget.IGNORE_AFZ \ + PositionTarget.IGNORE_YAW if (self.motion_type == 2): target_raw_pose.type_mask = PositionTarget.IGNORE_PX + PositionTarget.IGNORE_PY + PositionTarget.IGNORE_PZ \ + PositionTarget.IGNORE_VX + PositionTarget.IGNORE_VY + PositionTarget.IGNORE_VZ \ + PositionTarget.IGNORE_YAW return target_raw_pose
def set_target_pose(self,point,yaw, yaw_rate=1): target_pose = PositionTarget() target_pose.header.stamp = rospy.get_rostime() target_pose.coordinate_frame = 9 target_pose.position = point target_pose.yaw = yaw target_pose.yaw_rate = yaw_rate target_pose.type_mask = PositionTarget.IGNORE_VX + PositionTarget.IGNORE_VY + PositionTarget.IGNORE_VZ \ + PositionTarget.IGNORE_AFX + PositionTarget.IGNORE_AFY + PositionTarget.IGNORE_AFZ + PositionTarget.FORCE self.current_pose = point return target_pose