def mainloop(): rospy.init_node('roscopter') while not rospy.is_shutdown(): rospy.sleep(0.001) msg = master.recv_match(blocking=False) if not msg: continue #print msg.get_type() if msg.get_type() == "BAD_DATA": if mavutil.all_printable(msg.data): sys.stdout.write(msg.data) sys.stdout.flush() else: msg_type = msg.get_type() if msg_type == "RC_CHANNELS_RAW": pub_rc.publish([ msg.chan1_raw, msg.chan2_raw, msg.chan3_raw, msg.chan4_raw, msg.chan5_raw, msg.chan6_raw, msg.chan7_raw, msg.chan8_raw ]) if msg_type == "HEARTBEAT": pub_state.publish( msg.base_mode & mavutil.mavlink.MAV_MODE_FLAG_SAFETY_ARMED, msg.base_mode & mavutil.mavlink.MAV_MODE_FLAG_GUIDED_ENABLED, mavutil.mode_string_v10(msg)) if msg_type == "VFR_HUD": pub_vfr_hud.publish(msg.airspeed, msg.groundspeed, msg.heading, msg.throttle, msg.alt, msg.climb) if msg_type == "GPS_RAW_INT": fix = NavSatStatus.STATUS_NO_FIX if msg.fix_type >= 3: fix = NavSatStatus.STATUS_FIX pub_gps.publish( NavSatFix( latitude=msg.lat / 1e07, longitude=msg.lon / 1e07, altitude=msg.alt / 1e03, status=NavSatStatus(status=fix, service=NavSatStatus.SERVICE_GPS))) #pub.publish(String("MSG: %s"%msg)) if msg_type == "ATTITUDE": pub_attitude.publish(msg.roll, msg.pitch, msg.yaw, msg.rollspeed, msg.pitchspeed, msg.yawspeed) if msg_type == "LOCAL_POSITION_NED": print "Local Pos: (%f %f %f) , (%f %f %f)" % ( msg.x, msg.y, msg.z, msg.vx, msg.vy, msg.vz) if msg_type == "RAW_IMU": pub_raw_imu.publish(Header(), msg.time_usec, msg.xacc, msg.yacc, msg.zacc, msg.xgyro, msg.ygyro, msg.zgyro, msg.xmag, msg.ymag, msg.zmag)
def mainloop(): rospy.init_node('roscopter') while not rospy.is_shutdown(): rospy.sleep(0.001) msg = master.recv_match(blocking=False) if not msg: continue #print msg.get_type() if msg.get_type() == "BAD_DATA": if mavutil.all_printable(msg.data): sys.stdout.write(msg.data) sys.stdout.flush() else: msg_type = msg.get_type() if msg_type == "RC_CHANNELS_RAW" : pub_rc.publish([msg.chan1_raw, msg.chan2_raw, msg.chan3_raw, msg.chan4_raw, msg.chan5_raw, msg.chan6_raw, msg.chan7_raw, msg.chan8_raw]) if msg_type == "HEARTBEAT": pub_state.publish(msg.base_mode & mavutil.mavlink.MAV_MODE_FLAG_SAFETY_ARMED, msg.base_mode & mavutil.mavlink.MAV_MODE_FLAG_GUIDED_ENABLED, mavutil.mode_string_v10(msg)) if msg_type == "VFR_HUD": pub_vfr_hud.publish(msg.airspeed, msg.groundspeed, msg.heading, msg.throttle, msg.alt, msg.climb) if msg_type == "GPS_RAW_INT": fix = NavSatStatus.STATUS_NO_FIX if msg.fix_type >=3: fix=NavSatStatus.STATUS_FIX pub_gps.publish(NavSatFix(latitude = msg.lat/1e07, longitude = msg.lon/1e07, altitude = msg.alt/1e03, status = NavSatStatus(status=fix, service = NavSatStatus.SERVICE_GPS) )) #pub.publish(String("MSG: %s"%msg)) if msg_type == "ATTITUDE" : pub_attitude.publish(msg.roll, msg.pitch, msg.yaw, msg.rollspeed, msg.pitchspeed, msg.yawspeed) px4_time[0] = msg.time_boot_ms # get the time from px4 local_time[0] = time.time() # get the local start time if msg_type == "LOCAL_POSITION_NED" : print "Local Pos: (%f %f %f) , (%f %f %f)" %(msg.x, msg.y, msg.z, msg.vx, msg.vy, msg.vz) if msg_type == "RAW_IMU" : pub_raw_imu.publish (Header(), msg.time_usec, msg.xacc, msg.yacc, msg.zacc, msg.xgyro, msg.ygyro, msg.zgyro, msg.xmag, msg.ymag, msg.zmag)
def mainloop(): rospy.init_node('Mavlink_Node') while not rospy.is_shutdown(): rospy.sleep(0.001) msg = master.recv_match(blocking=False) if not msg: continue #print msg.get_type() if msg.get_type() == "BAD_DATA": if mavutil.all_printable(msg.data): sys.stdout.write(msg.data) sys.stdout.flush() else: msg_type = msg.get_type() if msg_type == "RC_CHANNELS_RAW" : pub_rc.publish([msg.chan1_raw, msg.chan2_raw, msg.chan3_raw, msg.chan4_raw, msg.chan5_raw, msg.chan6_raw, msg.chan7_raw, msg.chan8_raw]) if msg_type == "HEARTBEAT": pub_state.publish(msg.base_mode & mavutil.mavlink.MAV_MODE_FLAG_SAFETY_ARMED, msg.base_mode & mavutil.mavlink.MAV_MODE_FLAG_GUIDED_ENABLED, mavutil.mode_string_v10(msg)) if msg_type == "VFR_HUD": pub_vfr_hud.publish(msg.airspeed, msg.groundspeed, msg.heading, msg.throttle, msg.alt, msg.climb) if msg_type == "GPS_RAW_INT": #print "Got GPS Message. SV: %d eph: %d epv: %d" %(msg.satellites_visible,msg.eph,msg.epv) pub_gps_state.publish(msg.satellites_visible,msg.eph,msg.epv,msg.lat/1e07,msg.lon/1e07,msg.alt/1e03,msg.fix_type) fix = NavSatStatus.STATUS_NO_FIX if msg.fix_type >=3: fix=NavSatStatus.STATUS_FIX pub_gps.publish(NavSatFix(latitude = msg.lat/1e07, longitude = msg.lon/1e07, altitude = msg.alt/1e03, status = NavSatStatus(status=fix, service = NavSatStatus.SERVICE_GPS) )) #pub.publish(String("MSG: %s"%msg)) #if msg_type == "ATTITUDE" : #pub_attitude.publish(msg.roll, msg.pitch, msg.yaw, msg.rollspeed, msg.pitchspeed, msg.yawspeed) if msg_type == "LOCAL_POSITION_NED" : print "Local Pos: (%f %f %f) , (%f %f %f)" %(msg.x, msg.y, msg.z, msg.vx, msg.vy, msg.vz)
def mainloop(): global gps_msg, autonomous_enable rospy.init_node('casy_rover') if opts.enable_watchdog: rospy.Timer(rospy.Duration(1/opts.watchdog_rate), watchdog_timer_cb) # SEND IF YOU DESIRE A LIST OF ALL PARAMS (TODO: Publish params to a topic) #master.mav.param_request_list_send(master.target_system, master.target_component) # Permette di settare un parametro come in mission planner #master.param_set_send('FS_ACTION', 0, 2) # Riceve una lista di tutti i parametri disponibili sotto forma di messaggi PARAM_VALUE #master.param_fetch_all() r = rospy.Rate(opts.rate) while not rospy.is_shutdown(): r.sleep() msg = master.recv_match(blocking=False) if not msg: continue # Parse incoming message if msg.get_type() == "BAD_DATA": if mavutil.all_printable(msg.data): sys.stdout.write(msg.data) sys.stdout.flush() else: msg_type = msg.get_type() if msg_type == "RC_CHANNELS_RAW" : # If using Autonomous Control safety switch, use specified channel # to enable or disable autonomous control. Autonomous control # allows mode, rc, and waypoint controls. if (opts.enable_autonomous_safety_switch): if (msg.chan6_raw >= 1500): autonomous_enable = True elif (msg.chan6_raw < 1500): autonomous_enable = False # Give control back to controller master.mav.rc_channels_override_send(master.target_system, master.target_component, 0, 0, 0, 0, 0, 0, 0, 0) pub_rc.publish([msg.chan1_raw, msg.chan2_raw, msg.chan3_raw, msg.chan4_raw, msg.chan5_raw, msg.chan6_raw, msg.chan7_raw, msg.chan8_raw]) elif msg_type == "HEARTBEAT": pub_state.publish(msg.base_mode & mavutil.mavlink.MAV_MODE_FLAG_SAFETY_ARMED, msg.base_mode & mavutil.mavlink.MAV_MODE_FLAG_GUIDED_ENABLED, mavutil.mode_string_v10(msg)) state_msg.armed = msg.base_mode & mavutil.mavlink.MAV_MODE_FLAG_SAFETY_ARMED state_msg.mode = mavutil.mode_string_v10(msg) elif msg_type == "VFR_HUD": pub_vfr_hud.publish(msg.airspeed, msg.groundspeed, msg.heading, msg.throttle, msg.alt, msg.climb) elif msg_type == "GPS_RAW_INT": fix = NavSatStatus.STATUS_NO_FIX if msg.fix_type >=3: fix=NavSatStatus.STATUS_FIX header = Header() header.frame_id = 'base_link'# '/gps' header.stamp = rospy.Time.now() #rospy.loginfo("Hdop is %d", msg.eph) #rospy.loginfo("Vdop is %d", msg.epv) sigma = math.sqrt((3.04 * msg.eph**2)**2 + 3.57**2) position_covariance = [0] * 9 position_covariance[0] = sigma #9999 position_covariance[4] = sigma #9999 position_covariance[8] = sigma #9999 pub_gps.publish(NavSatFix(header = header, latitude = msg.lat/1e07, longitude = msg.lon/1e07, altitude = msg.alt/1e03, position_covariance=position_covariance, position_covariance_type=NavSatFix.COVARIANCE_TYPE_APPROXIMATED, status = NavSatStatus(status=fix, service = NavSatStatus.SERVICE_GPS) )) gps_msg.latitude = msg.lat gps_msg.longitude = msg.lon elif msg_type == "LOCAL_POSITION_NED" : rospy.loginfo("Local Pos: (%f %f %f) , (%f %f %f)" %(msg.x, msg.y, msg.z, msg.vx, msg.vy, msg.vz)) elif msg_type == "RAW_IMU" : pub_raw_imu.publish (Header(), msg.time_usec, msg.xacc, msg.yacc, msg.zacc, msg.xgyro, msg.ygyro, msg.zgyro, msg.xmag, msg.ymag, msg.zmag) elif msg_type == "SYS_STATUS": status_msg = casy_rover.msg.Status() status_msg.header.stamp = rospy.Time.now() status_msg.battery_voltage = msg.voltage_battery status_msg.battery_current = msg.current_battery status_msg.battery_remaining = msg.battery_remaining status_msg.sensors_enabled = msg.onboard_control_sensors_enabled pub_status.publish(status_msg) elif msg_type == "GLOBAL_POSITION_INT": header = Header() header.stamp = rospy.Time.now() filtered_pos_msg.header = header filtered_pos_msg.latitude = msg.lat filtered_pos_msg.longitude = msg.lon filtered_pos_msg.altitude = msg.alt filtered_pos_msg.relative_altitude = msg.relative_alt filtered_pos_msg.ground_x_speed = msg.vx filtered_pos_msg.ground_y_speed = msg.vy filtered_pos_msg.ground_z_speed = msg.vz filtered_pos_msg.heading = msg.hdg pub_filtered_pos.publish(filtered_pos_msg) elif msg_type == "COMMAND_ACK": rospy.loginfo ("COMMAND_ACK: Command Message ACK with result - " + str(msg.result)) elif msg_type == "STATUSTEXT": rospy.loginfo ("STATUSTEXT: Status severity is %d. Text Message is %s" %(msg.severity, msg.text)) elif msg_type == "PARAM_VALUE": rospy.loginfo ("PARAM_VALUE: ID = %s, Value = %d, Type = %d, Count = %d, Index = %d" %(msg.param_id, msg.param_value, msg.param_type, msg.param_count, msg.param_index))