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 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 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 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 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 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 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 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 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 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 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 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 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 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 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 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 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 main(): while True: rospy.Subscriber('/mavros/local_position/pose', PoseStamped, callback_pos) local_pos_pub = rospy.Publisher('/mavros/setpoint_raw/local', PositionTarget, queue_size=10) pose = PositionTarget() pose.position.x = 0.9 pose.velocity.z = 0 pose.acceleration_or_force.z = 0 global i t0 = rospy.Time.now().to_sec( ) # To just wander left and ri8 to get a detection while rospy.Time.now().to_sec() - t0 < 2: # its enough i think # if i%2 == 0: # pose.position.y = 1 # else: # pose.position.y = -1 pose.position.y = 0.2 pose.position.z = 3 local_pos_pub.publish(pose) i = i + 1
def __init__(self): # Communication variables self._last_pose = PoseStamped() self._setpoint_msg = PositionTarget() self._setpoint_msg.type_mask = _DEFAULT self._last_state = State() # ROS Communication self._setpoint_pub = rospy.Publisher("mavros/setpoint_raw/local", PositionTarget, queue_size=1) self._pose_sub = rospy.Subscriber("mavros/local_position/pose", PoseStamped, self._pose_cb) self._state_sub = rospy.Subscriber('mavros/state', State, self._state_cb) # wait for mavros to start publishing rospy.logdebug("Waiting for MAVROS to start") rospy.wait_for_message("mavros/mission/waypoints", WaypointList) # Make drone less aggressive rospy.wait_for_service("mavros/param/set") mavparam = rospy.ServiceProxy('mavros/param/set', ParamSet) mavparam("MC_PITCH_P", ParamValue(0, 2.0)).success mavparam("MC_ROLL_P", ParamValue(0, 2.0)).success mavparam("MPC_XY_VEL_MAX", ParamValue(0, 3.0)).success # Send a few setpoint to make MAVROS happy rate = rospy.Rate(20.0) for _ in range(40): rate.sleep() self._publish_setpoint(None) rospy.Timer(rospy.Duration(0.05), self._publish_setpoint) rospy.loginfo("Drone Initialized")
def __init__(self): self.state = "disarm" #создаем топики, для публикации управляющих значений: self.pub_pt = rospy.Publisher(f"/mavros{1}/setpoint_raw/local", PositionTarget, queue_size=10) self.pt = PositionTarget() self.pt.coordinate_frame = self.pt.FRAME_LOCAL_NED self.t0 = time.time() self.dt = 0 #params self.p_gain = 2 self.max_velocity = 5 self.arrival_radius = 0.6 self.waypoint_list = [ np.array([6., 7., 6.]), np.array([0., 14., 7.]), np.array([18., 14., 7.]), np.array([0., 0., 5.]) ] self.current_waypoint = np.array([0., 0., 0.]) self.pose = np.array([0., 0., 0.]) self.velocity = np.array([0., 0., 0.]) self.mavros_state = State() self.subscribe_on_topics()
def __init__( self, flight_instruction_type='generic_mission_state', timeout=10, mavros_message_node=None, parent_ref=None, # tol_heading_deg=1.0, state_label="unspecified", # use this to store information about the purpose of the state, e.g. "explore the world" timeout_OK=True, # this state should be set to false if timouts should be considered a mission failure tol_distance=0.2, tol_heading_deg=5, **kwargs): self.flight_instruction_type = flight_instruction_type self._timeout = timeout self._setpoint_raw = PositionTarget() self.state_label = state_label self.timeout_OK = timeout_OK self.mission_state_busy = False # this flag is used to indicate that the flight state is still processing an instruction and should not be polled again # Initialise mission state setpoint with the previous states setpoint self.update_sp_locals() # initialise local properties self._ros_message_node = mavros_message_node self._parent_ref = parent_ref self.stay_alive = True self.preconditions_satisfied = False self._prerun_complete = False self.tol_distance = tol_distance self.tol_heading_deg = tol_heading_deg self.tol_heading_rad = np.deg2rad(tol_heading_deg)
def __init__(self): # Communication variables self._last_pose = None self._last_twist = None self._setpoint_msg = PositionTarget() self._last_state = State() self._camera = Camera(imgtopic="drone/camera_front/image", infotopic="drone/camera_front/info") # ROS Communication self._setpoint_pub = rospy.Publisher("mavros/setpoint_raw/local", PositionTarget, queue_size=1) self._pose_sub = rospy.Subscriber("mavros/local_position/pose", PoseStamped, self._pose_cb) self._twist_sub = rospy.Subscriber("mavros/local_position/velocity", TwistStamped, self._twist_cb) self._state_sub = rospy.Subscriber('mavros/state', State, self._state_cb) self._publish_setpoint_active = False rospy.Timer(rospy.Duration(0.05), self._publish_setpoint) rospy.loginfo("Drone Initialized")
def __init__(self, altitude = flight_alt): rospy.init_node('quadsim_teleop') # creates the node self.state_sub = rospy.Subscriber("%s/mavros/state" % uav, State, self.state_cb) self.local_pose_pub = rospy.Publisher("%s/mavros/setpoint_position/local" % uav, PoseStamped, queue_size=10) self.landing = False # Clients self.arm_client = rospy.ServiceProxy("%s/mavros/cmd/arming" % uav, CommandBool) self.land_client = rospy.ServiceProxy("%s/mavros/cmd/land" % uav, CommandTOL) self.land_sub = rospy.Subscriber('/quadcopter_land', Empty, self.land_cb) # Listens for a command to land drone self.current_state = None self.des_z = altitude self.rate = rospy.Rate(20) self.arm() # subscribes to cmd_vel to receive velocity commands self.sub = rospy.Subscriber('/cmd_vel', Twist, self.twist_cb) # publishes velocity commands to hover and teleop self.pub = rospy.Publisher('%s/mavros/setpoint_raw/local' % uav, PositionTarget, queue_size=3) # define msg self.setvel_msg = PositionTarget() self.define_msg()
def __init__(self, vehicle_id): self.vehicle_type = 'plane' self.vehicle_id = vehicle_id self.local_pose = None self.target_motion = PositionTarget() self.arm_state = False self.motion_type = 0 self.flight_mode = None self.mission = None ''' ros subscribers ''' self.local_pose_sub = rospy.Subscriber(self.vehicle_type+'_'+self.vehicle_id+"/mavros/local_position/pose", PoseStamped, self.local_pose_callback) self.cmd_pose_flu_sub = rospy.Subscriber("/xtdrone/"+self.vehicle_type+'_'+self.vehicle_id+"/cmd_pose_flu", Pose, self.cmd_pose_flu_callback) self.cmd_pose_enu_sub = rospy.Subscriber("/xtdrone/"+self.vehicle_type+'_'+self.vehicle_id+"/cmd_pose_enu", Pose, self.cmd_pose_enu_callback) self.cmd_sub = rospy.Subscriber("/xtdrone/"+self.vehicle_type+'_'+self.vehicle_id+"/cmd",String,self.cmd_callback) ''' ros publishers ''' self.target_motion_pub = rospy.Publisher(self.vehicle_type+'_'+self.vehicle_id+"/mavros/setpoint_raw/local", PositionTarget, queue_size=10) self.odom_groundtruth_pub = rospy.Publisher('/xtdrone/'+self.vehicle_type+'_'+self.vehicle_id+'/ground_truth/odom', Odometry, queue_size=10) ''' ros services ''' self.armService = rospy.ServiceProxy(self.vehicle_type+'_'+self.vehicle_id+"/mavros/cmd/arming", CommandBool) self.flightModeService = rospy.ServiceProxy(self.vehicle_type+'_'+self.vehicle_id+"/mavros/set_mode", SetMode) self.gazeboModelstate = rospy.ServiceProxy('/gazebo/get_model_state', GetModelState) print("communication initialized")