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 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 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 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 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 set_vel_yaw(self, vel, yaw, freq): """ Offboard method that sends velocity and yaw 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. Parameters ---------- vel : np.array of floats with shape (3,1) Desired linear velocity for drone, in local NED coordinates. yaw : float Desired yaw for the vehicle, in radians. 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 = 1 msg.type_mask = 3015 msg.velocity.x, msg.velocity.y, msg.velocity.z = ned_to_enu(vel) msg.yaw = -yaw pub.publish(msg) rate = rospy.Rate(freq) rate.sleep()
def Aruco_marker_detector(self): """ Use the build in python library to detect the aruco marker and its coordinates """ img = self.frame.copy() # Detect the markers in the image markerCorners, markerIds, rejectedCandidates = cv.aruco.detectMarkers(img, self.dictionary, parameters=self.parameters) # Get the transformation matrix to the marker if markerCorners: markerSize = 2.0 axisLength = 3.0 rvecs, tvecs, _ = cv.aruco.estimatePoseSingleMarkers(markerCorners, markerSize, self.K, self.distCoeffs) # Draw the axis on the marker frame_out = cv.aruco.drawAxis( img, self.K, self.distCoeffs, rvecs, tvecs, axisLength) rotMat = tr.euler_matrix(np.pi / 2.0, 0, 0) rotMat = rotMat[0:3, 0:3] tvecs = np.matmul(rotMat, tvecs[0][0].T) rotMat, _ = cv.Rodrigues(rvecs) eul = tr.euler_from_matrix(rotMat) yaw_angle = self.pos[3] - eul[2] # Publish the position of the marker marker_pos = PT() marker_pos.position.x = tvecs[0] marker_pos.position.y = - tvecs[2] marker_pos.position.z = tvecs[1] marker_pos.yaw = eul[2] * np.pi / 180.0 self.aruco_marker_pos_pub.publish(marker_pos) # Publish the image with the detected marker self.aruco_marker_img_pub.publish(self.bridge.cv2_to_imgmsg(frame_out, "bgr8"))
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 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 set_pose(self, x=0, y=0, z=2, yaw=0, BODY_OFF_SET_ENU=True): pose = PositionTarget() pose.header.stamp = rospy.Time.now() if BODY_OFF_SET_ENU: pose.header.frame_id = 'frame.body' pose.position.x = x pose.position.y = y pose.position.z = z pose.yaw = yaw else: pose.header.frame_id = 'frame.local_enu' pose.position.x = x pose.position.y = y pose.position.z = z pose.yaw = yaw return pose
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 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 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 listener(): rospy.init_node('listener', anonymous=True) rate = rospy.Rate(20) rospy.Subscriber('mavros/state', State, callback) local_pos_pub = rospy.Publisher('mavros/setpoint_position/local', PoseStamped, queue_size=2) local_posi_raw_pub = rospy.Publisher('mavros/setpoint_raw/local', PositionTarget, queue_size=2) #local_vel_pub=rospy.Publisher('mavros/setpoint_position/local',PoseStamped,queue_size=2) set_mode_cmd = rospy.ServiceProxy('mavros/set_mode', SetMode) arm_cmd = rospy.ServiceProxy('mavros/cmd/arming', CommandBool) newpos = PoseStamped() newpos.pose.position.z = 10.0 #=Point(0,0,2) #set_mode_cmd('POSITION CONTROL','') #for _ in range(10): # rate.sleep() newvel = PositionTarget() newvel.velocity.x = -0.5 newvel.yaw = -90.0 / 180.0 * 3.142 newvel.type_mask = newvel.FRAME_LOCAL_NED | newvel.IGNORE_AFX | newvel.IGNORE_AFY | newvel.IGNORE_AFZ newvel.type_mask = newvel.type_mask | newvel.IGNORE_PX | newvel.IGNORE_PY | newvel.IGNORE_PZ for _ in range(100): rate.sleep() local_pos_pub.publish(newpos) mymode = 'OFFBOARD' last_req = rospy.get_time() start_time = last_req #print '---',rospy.get_time(),start_time while rospy.get_time() - start_time < 70: if rospy.get_time() - last_req > 5: if state.mode != mymode: set_mode_cmd(0, mymode) rospy.loginfo('setting mode...') elif not state.armed: arm_cmd(True) rospy.loginfo('arming...') last_req = rospy.get_time() dt = rospy.get_time() - start_time if dt < 20: local_pos_pub.publish(newpos) elif dt < 40: local_posi_raw_pub.publish(newvel) elif dt < 60: newvel.velocity.x = 0.5 local_posi_raw_pub.publish(newvel) else: newpos.pose.position.z = 0 local_pos_pub.publish(newpos) rate.sleep()
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 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 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
def pose_publisher_line(): pub = rospy.Publisher('/mavros/setpoint_raw/local', PositionTarget, queue_size=10) pose_msg = PositionTarget() #pose_msg.model_name = 'car_landing_pad' rate = rospy.Rate(100) linear_vel = 1.0 time = 0.0 while not rospy.is_shutdown(): #pose_msg.header.stamp = rospy.Time.now() #pose_msg.header.frame_id = "home" pose_msg.coordinate_frame = 1 pose_msg.position.x = 2.0 pose_msg.position.y = 2.0 pose_msg.position.z = 0.0 pose_msg.velocity.x = 0.2 pose_msg.velocity.y = 0.2 pose_msg.velocity.z = 0.0 pose_msg.yaw = 0.0 pose_msg.yaw_rate = 0.0 if (motion_type == 0): pose_msg.type_mask = PositionTarget.IGNORE_VX + PositionTarget.IGNORE_VY + PositionTarget.IGNORE_VZ \ + PositionTarget.IGNORE_AFX + PositionTarget.IGNORE_AFY + PositionTarget.IGNORE_AFZ + PositionTarget.IGNORE_YAW_RATE pose_msg.type_mask = 504 print('Pos_x :', pose_msg.position.x) print('Pos_y :', pose_msg.position.y) print('Pos_z :', pose_msg.position.z) print('Yaw :', pose_msg.yaw) if (motion_type == 1): pose_msg.type_mask = PositionTarget.IGNORE_PX + PositionTarget.IGNORE_PY + PositionTarget.IGNORE_PZ \ + PositionTarget.IGNORE_AFX + PositionTarget.IGNORE_AFY + PositionTarget.IGNORE_AFZ + PositionTarget.IGNORE_YAW_RATE print('Vel_x :', pose_msg.velocity.x) print('Vel_y :', pose_msg.velocity.y) print('Vel_z :', pose_msg.velocity.z) print('Yaw :', pose_msg.yaw) pub.publish(pose_msg) rate.sleep()
def send_commands(self, event): 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.ignore_mask waypoint_command_msg.position.x = 0.0 # E waypoint_command_msg.position.y = 0.0 # N waypoint_command_msg.position.z = 15.0 # U yaw = -self.heading + 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.publish(waypoint_command_msg)
def set_Local_Waypoint(x_Pos, y_Pos, travel_Height, x_Vel, y_Vel, z_Vel, yaw_Angle): """ This function sets the local waypoint using ENU notation. x_Pos: East - positive position in meters from home position y_Pos: North - positive position in meters from home position z_Pos: Up - positive position in meters from the home position x_Vel, y_Vel, z_Vel: Corresponding velocities. Not currently working properly. yaw_Angle: Desired yaw, in radians, as measured counterclockwise from North """ takeoff_Waypoint = PositionTarget() #Timestamp message takeoff_Waypoint.header.stamp = rospy.get_rostime() #Set reference frame as global takeoff_Waypoint.header.frame_id = "1" #Local ENU coordinate system takeoff_Waypoint.coordinate_frame = 1 #Taken from ardupilot documentation. #Sets acceleration and force control bits to 1, or inactive #Sets position, velocity, and yaw bits to 0, or active, allowing for # control of these elements. Decimal equivalent is 3008. takeoff_Waypoint.type_mask = 0b0000101111000000 #set desired positions and travel velocities takeoff_Waypoint.position.x = x_Pos takeoff_Waypoint.position.y = y_Pos takeoff_Waypoint.position.z = travel_Height takeoff_Waypoint.velocity.x = x_Vel takeoff_Waypoint.velocity.y = y_Vel takeoff_Waypoint.velocity.z = z_Vel takeoff_Waypoint.yaw = yaw_Angle return takeoff_Waypoint
def _ConstructTarget(self, x, y, z, yaw, yaw_rate=1): target_raw_pose = PositionTarget() target_raw_pose.header.stamp = rospy.Time.now() # uint8 coordinate_frame # FRAME_LOCAL_NED = 1 # FRAME_LOCAL_OFFSET_NED = 7 # FRAME_BODY_NED = 8 # FRAME_BODY_OFFSET_NED = 9 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_commands(self, msg): time_cur = rospy.get_time() self.ibvs_active_pub_.publish(self.ibvs_active_msg) # reset the ibvs counter if we haven't seen the ArUco for more than 1 second if time_cur - self.ibvs_time >= 1.0 and time_cur - self.ibvs_time_inner >= 1.0 and self.counters_frozen == False: self.ibvs_count = 0 self.ibvs_count_inner = 0 self.ibvs_active_msg.data = False # print "reset" # if the ArUco has been in sight for a while if self.ibvs_count > self.count_outer_req or self.ibvs_count_inner > self.count_inner_req: self.ibvs_active_msg.data = True if self.ibvs_count_inner > self.count_inner_req: print(self.distance) # in this case we enter an open-loop drop onto the marker if self.distance < 1.0 and self.distance > 0.1 and self.land_mode_sent == False: # set the PX4 to land mode self.execute_landing() # freeze the counters # self.counters_frozen = True # ibvs_command_msg = Command() # ibvs_command_msg.x = 0.0 # ibvs_command_msg.y = 0.0 # ibvs_command_msg.F = 0.0 # ibvs_command_msg.z = 0.0 # ibvs_command_msg.mode = Command.MODE_ROLL_PITCH_YAWRATE_THROTTLE # self.waypoint_pub_.publish(ibvs_command_msg) # # stuff else: ibvs_command_msg = PositionTarget() ibvs_command_msg.header.stamp = rospy.get_rostime() ibvs_command_msg.coordinate_frame = ibvs_command_msg.FRAME_BODY_NED ibvs_command_msg.type_mask = self.velocity_mask ibvs_command_msg.velocity.x = self.saturate( self.ibvs_x_inner, self.u_max_inner, -self.u_max_inner) ibvs_command_msg.velocity.y = self.saturate( self.ibvs_y_inner, self.v_max_inner, -self.v_max_inner) ibvs_command_msg.velocity.z = self.saturate( self.ibvs_F_inner, self.w_max_inner, -self.w_max_inner) ibvs_command_msg.yaw_rate = self.ibvs_z_inner # Publish. self.command_pub.publish(ibvs_command_msg) if self.ibvs_count_inner > 1000: self.ibvs_count_inner = self.count_inner_req + 1 # reset so it doesn't get too big else: ibvs_command_msg = PositionTarget() ibvs_command_msg.header.stamp = rospy.get_rostime() ibvs_command_msg.coordinate_frame = ibvs_command_msg.FRAME_BODY_NED ibvs_command_msg.type_mask = self.velocity_mask ibvs_command_msg.velocity.x = self.saturate( self.ibvs_x, self.u_max, -self.u_max) ibvs_command_msg.velocity.y = self.saturate( self.ibvs_y, self.v_max, -self.v_max) ibvs_command_msg.velocity.z = self.saturate( self.ibvs_F, self.w_max, -self.w_max) ibvs_command_msg.yaw_rate = self.ibvs_z # Publish. self.command_pub.publish(ibvs_command_msg) if self.ibvs_count > 1000: self.ibvs_count = self.count_outer_req + 1 # reset so it doesn't get too big # go back to following regular waypoints else: # print "following waypoints" 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.wp_D # U waypoint_command_msg.yaw = np.radians(0.0) # Point North? # Publish. self.command_pub.publish(waypoint_command_msg)
try: rospy.init_node('eksen',anonymous=True) global msg pub = rospy.Publisher('/mavros/setpoint_raw/local', PositionTarget,queue_size=10) rospy.Subscriber('/mavros/local_position/odom', Odometry, call_back2) rospy.Subscriber('konum', String, call_back) msg.header.stamp = rospy.Time.now() msg.header.frame_id= "world" msg.coordinate_frame = 8 msg.type_mask=2503 msg.velocity.x = 0.0 msg.velocity.y = 0.0 msg.velocity.z = 0.0 msg.yaw = 0.0 while not rospy.is_shutdown(): pub.publish(msg) except rospy.ROSInterruptException: pass
#近地点不控制 if feedback_z < 0.05: output_x = 0.0 output_y = 0.0 # 自由落体测试,整定速度PID专用MPC_Z_VEL_* # if feedback_z > 0.3: # set_z = 1 # if set_z: # output_z = -0.08 # output_y = 0 # output_x = 0 raw_msg.velocity.x = output_x raw_msg.velocity.y = output_y raw_msg.velocity.z = output_z raw_msg.yaw = output_yaw if enable_setpoint: position_pub.publish(raw_msg) tag_fuse = PoseStamped() tag_fuse.header.stamp = stamp tag_fuse.pose.position.x = tag_fuse_x tag_fuse.pose.position.y = tag_fuse_y position_tag.publish(tag_fuse) #outter loop print "x_set:", pid_x.SetPoint, "x_out:", output_x, "px_Feb:", feedback_x, "vx_Feb", local_linear_x print "y_set:", pid_y.SetPoint, "y_out:", output_y, "py_Feb:", feedback_y, "vy_Feb", local_linear_y print "z_set:", pid_z.SetPoint, "z_out:", output_z, "pz_Feb:", feedback_z, "vz_Feb", local_linear_z, 'yaw_set', output_yaw, 'yaw_feb', odom_yaw, 'pitch_feb', odom_pitch, 'roll_feb', odom_roll rate.sleep()
current_state = state current_pose = PoseStamped() def pos_cb(poseStamped): global current_pose current_pose = poseStamped # Commaded Setpoint setpoint = PositionTarget() setpoint.coordinate_frame = PositionTarget.FRAME_LOCAL_NED setpoint.type_mask = PositionTarget.IGNORE_AFX | PositionTarget.IGNORE_AFY | PositionTarget.IGNORE_AFZ | PositionTarget.IGNORE_YAW_RATE setpoint.yaw = 0 # distance function def DistToGoal(): return math.dist([ current_pose.pose.position.x, current_pose.pose.position.y, current_pose.pose.position.z ], [setpoint.position.x, setpoint.position.y, setpoint.position.z]) # Hoops Lookup Table hoops = [1, 0, 2, -1, -1.5, 0.5, -1.5, 1.5, -1.5, 1, -0.5, 2, 0, -1, 1] counter = 0 y_off = -0.10 x_off = 0.05
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.platform.split('-')[-2] == '16.04': 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 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.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.acceleration_or_force.x = afx target_raw_pose.acceleration_or_force.y = afy target_raw_pose.acceleration_or_force.z = afz elif self.platform.split('-')[-2] == '18.04': 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 else: print('XTDrone only supports Ubuntu16.04 and Ubuntu18.04 now.') sys.exit(0) 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 build_position_target(px=None, py=None, pz=None, v=None, vx=None, vy=None, vz=None, a=None, ax=None, ay=None, az=None, yaw=None, yaw_rate=None, is_force=False): msg = PositionTarget() msg.header.stamp = rospy.Time.now() msg.type_mask = ALL_FLAGS msg.coordinate_frame = PositionTarget.FRAME_LOCAL_NED if v is not None: vx, vy, vz = v, v, v if a is not None: ax, ay, az = a, a, a if px is not None: msg.position.x = px msg.type_mask -= PositionTarget.IGNORE_PX if py is not None: msg.position.y = py msg.type_mask -= PositionTarget.IGNORE_PY if pz is not None: msg.position.z = pz msg.type_mask -= PositionTarget.IGNORE_PZ if vx is not None: msg.velocity.x = vx msg.type_mask -= PositionTarget.IGNORE_VX if vy is not None: msg.velocity.y = vy msg.type_mask -= PositionTarget.IGNORE_VY if vz is not None: msg.velocity.z = vz msg.type_mask -= PositionTarget.IGNORE_VZ if ax is not None: msg.velocity.x = ax msg.type_mask -= PositionTarget.IGNORE_AFX if ay is not None: msg.velocity.y = ay msg.type_mask -= PositionTarget.IGNORE_AFY if az is not None: msg.velocity.z = az msg.type_mask -= PositionTarget.IGNORE_AFZ if yaw is not None: msg.yaw = yaw msg.type_mask -= PositionTarget.IGNORE_YAW if yaw_rate is not None: msg.yaw_rate = yaw_rate msg.type_mask -= PositionTarget.IGNORE_YAW_RATE if is_force: msg.type_mask -= PositionTarget.FORCE return msg
att_cmd.velocity = Vector3() att_cmd.velocity.x = 0 att_cmd.velocity.y = 0 att_cmd.velocity.z = 0.5 for i in range(0, 100): set_raw_local.publish(att_cmd) rate.sleep() set_mode("OFFBOARD", 5) set_arm(True, 5) att_cmd.velocity.x = 0 att_cmd.velocity.y = 0 att_cmd.velocity.z = 0.1 att_cmd.yaw = 0 while not rospy.is_shutdown(): if not q.empty(): key = q.get() # Left and right. if key == "d": att_cmd.velocity.x += 0.1 if key == "q": att_cmd.velocity.x -= 0.1 # Forward and backwards. if key == "z": att_cmd.velocity.y += 0.1 if key == "s":
set_mode = rospy.ServiceProxy(mavros.get_topic('set_mode'), mavros_msgs.srv.SetMode) set_mode(0,'OFFBOARD') counter = 0 try: while not rospy.is_shutdown(): pub.publish(msg) counter= counter + 1 if( counter>=50): msg.type_mask ^= PositionTarget.IGNORE_YAW msg.yaw = math.pi print counter if( counter>=100): set_mode(0,'AUTO.LAND') rate.sleep() except rospy.ROSInterruptException: pass