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))
