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 nanokontrolCallback(self,msg):
velocity = (((msg.axes[0])+1)*4)
param = ParamValue()
param.real = velocity
paramReq = ParamSetRequest()
paramReq.param_id = 'MPC_XY_CRUISE'
paramReq.value = param
self.param_service.call(paramReq)
def setSIM_BLOCK_GPS(self, setdata):
rospy.wait_for_service('/mavros/param/set')
try:
myparam = ParamValue()
myparam.integer = setdata
out = self.change_param("SIM_GPS_BLOCK", myparam)
except rospy.ServiceException as e:
print(e)
rospy.loginfo("Service call failed")
def set_param(self, parameter, value, timeout):
"""set Parameters: , timeout(int): seconds"""
param_get = False
param_set = False
prev_param = 0
now_param = 0
# Get prev_param
# rospy.loginfo("get FCU parameter: {0} to {1}".format(parameter, value))
loop_freq = 2 # Hz
rate = rospy.Rate(loop_freq)
for i in xrange(timeout * loop_freq):
# Get parameter
if False == param_get:
try:
res = self.get_param_srv(parameter)
if res.success == True:
if res.value.integer != 0:
prev_param = res.value.integer
else:
prev_param = res.value.real
param_get = True
except rospy.ServiceException as e:
rospy.logerr(e)
# Set pararmeter
if (True == param_get) & (False == param_set):
try:
req = ParamValue(0, 0.)
if type(value) == int:
req = ParamValue(value, 0.0)
else:
req = ParamValue(0, value)
res = self.set_param_srv(parameter, req)
if res.success == True:
if res.value.integer != 0:
now_param = res.value.integer
else:
now_param = res.value.real
rospy.loginfo(
"Param changed | {0} : {1:.4} -> {2:.4} | seconds: {3} of {4}"
.format(parameter, prev_param, now_param,
i / loop_freq, timeout))
param_set = True
except rospy.ServiceException as e:
rospy.logerr(e)
# All Done?
if (param_set == True) & (param_get == True):
break
# sleep
try:
rate.sleep()
except rospy.ROSException as e:
rospy.logfatal(e)
# Didn't you do that?
if (param_set == False) | (param_get == False):
rospy.logerr(
"failed to set parameter | param {0} | timeout(seconds): {1}".
format(parameter, timeout))
def set_servo9_max(rate):
# TODO Figure out exceptable range for this
# maximum PWM pulse width in microseconds must be less than 32767.
rospy.wait_for_service('/mavros/param/set')
try:
set_rate = rospy.ServiceProxy('/mavros/param/set', ParamSet)
value = ParamValue()
value.integer = rate
resp1 = set_rate('SERVO9_MAX', value)
except rospy.ServiceException as e:
rospy.logerr("+++++ TRIGGER: Service call failed: %s"%e)
def nanokontrolCallback(self,msg):
for key,value in self.paramList.items():
paramVal = (((msg.axes[value])+1)*2.0)
print paramVal
param = ParamValue()
param.real = paramVal
paramReq = ParamSetRequest()
paramReq.param_id = key
paramReq.value = param
print key, value, paramVal
self.param_service.call(paramReq)
def set_servo_rate(rate):
# sets the pulse frequency in hertz
# Minimum work value 16 tested up to 50
rospy.wait_for_service('/mavros/param/set')
try:
set_rate = rospy.ServiceProxy('/mavros/param/set', ParamSet)
value = ParamValue()
value.integer = rate
resp1 = set_rate('SERVO_RATE', value)
except rospy.ServiceException as e:
rospy.logerr("+++++ TRIGGER: Service call failed: %s"%e)
def nanokontrolCallback(self, msg):
for key, value in self.paramList.items():
paramVal = (((msg.axes[value]) + 1) * 2.0)
print paramVal
param = ParamValue()
param.real = paramVal
paramReq = ParamSetRequest()
paramReq.param_id = key
paramReq.value = param
print key, value, paramVal
self.param_service.call(paramReq)
def set_param(self, param, value):
proper_value = ParamValue()
proper_value.integer = value
rospy.wait_for_service(self.uav_id + '/mavros/param/set')
try:
set_fcu_param = rospy.ServiceProxy(
self.uav_id + '/mavros/param/set', ParamSet)
resp = set_fcu_param(param, proper_value)
return resp.success
except rospy.ServiceException, e:
print "Param Set Service call failed: %s" % e
def param_set(self, param, value):
if isinstance(value, float):
val = ParamValue(integer=0, real=value)
else:
val = ParamValue(integer=value, real=0.0)
rospy.wait_for_service(self.ns + '/mavros/param/set')
try:
set_param = rospy.ServiceProxy(self.ns + '/mavros/param/set',
ParamSet)
resp = set_param(param_id=param, value=val)
print("setmode send ok", resp.success)
except rospy.ServiceException as e:
print("Failed SetMode:", e)
def establish_QGC_control():
myparam = ParamValue()
# To give control back to QGoundControl
rospy.wait_for_service('/mavros/param/set')
try:
myparam.integer = 255
myparam.real = 0
out = change_param("SYSID_MYGCS", myparam)
if out.success:
rospy.loginfo("QGroundControl control established")
else:
rospy.loginfo("Failed gaining QGroundControl control")
except rospy.ServiceException, e:
rospy.loginfo("Service call failed")
def establish_ROS_control():
myparam = ParamValue()
# To have control from ROS
rospy.wait_for_service('/mavros/param/set')
try:
myparam.integer = 1
myparam.real = 0
out = change_param("SYSID_MYGCS", myparam)
if out.success:
rospy.loginfo("ROS control esablished")
else:
rospy.loginfo("Failed gaining ROS control")
except rospy.ServiceException, e:
rospy.loginfo("Service call failed")
def param_set(param_id, value):
if isinstance(value, float):
val = ParamValue(integer=0, real=value)
else:
val = ParamValue(integer=value, real=0.0)
try:
set = rospy.ServiceProxy(mavros.get_topic('param', 'set'), ParamSet)
ret = set(param_id=param_id, value=val)
except rospy.ServiceException as ex:
raise IOError(str(ex))
if not ret.success:
raise IOError("Request failed.")
return param_ret_value(ret)
def test_posctl(self):
"""Test offboard position control"""
# make sure the simulation is ready to start the mission
self.wait_for_topics(60)
self.wait_for_landed_state(mavutil.mavlink.MAV_LANDED_STATE_ON_GROUND,
10, -1)
self.log_topic_vars()
# exempting failsafe from lost RC to allow offboard
rcl_except = ParamValue(1<<2, 0.0)
self.set_param("COM_RCL_EXCEPT", rcl_except, 5)
self.set_mode("OFFBOARD", 5)
self.set_arm(True, 5)
rospy.loginfo("run mission")
positions = ((0, 0, 0), (50, 50, 20), (50, -50, 20), (-50, -50, 20),
(0, 0, 20))
for i in xrange(len(positions)):
self.reach_position(positions[i][0], positions[i][1],
positions[i][2], 30)
self.set_mode("AUTO.LAND", 5)
self.wait_for_landed_state(mavutil.mavlink.MAV_LANDED_STATE_ON_GROUND,
45, 0)
self.set_arm(False, 5)
def set_int_param(param, value):
try:
val = ParamValue()
val.integer = value
val.real = 0.0
ret = set_param(param, val)
except rospy.ServiceException as e:
logexc('Set param failed')
return False
if not ret.success:
rospy.logerr("Set param failed: unknown")
return False
return True
def __init__(self, name='apm_marshall'):
super(APMMarshallNode, self).__init__(name)
self.prev_channels = []
self.name = rospy.get_name()
rospy.loginfo('[%s] waiting for /mavros/param/set...' % self.name)
rospy.wait_for_service('/mavros/param/set')
self.set_param_client = rospy.ServiceProxy('/mavros/param/set',
ParamSet)
self.get_param_client = rospy.ServiceProxy('/mavros/param/get',
ParamGet)
# get defaults
self.params = dict(SYSID_MYGCS=ParamValue(1, 0.0),
ARMING_REQUIRE=ParamValue(0, 0.0),
ARMING_CHECK=ParamValue(0, 0.0))
self.default_params = {}
self.get_param('SYSID_MYGCS')
rospy.loginfo('[%s] waiting for parameters to be downloaded...' %
self.name)
for p in self.params:
self.default_params[p] = self.get_param(p)
self.cmd_pub = rospy.Publisher('/mavros/rc/override',
OverrideRCIn,
queue_size=1)
self.rc_sub = rospy.Subscriber('/mavros/rc/in', RCIn,
self.rc_in_callback)
if self.FSM in [FSM_STATES.USER, FSM_STATES.USER_PROMPT]:
self.set_user_mode()
else:
self.set_rl_mode()
self.fsm_pub.publish(str(self.FSM))
self.rc_in_rl_sub = rospy.Subscriber('/sandbox/rc/override',
OverrideRCIn,
self.process_command)
self.mavros_state_sub = rospy.Subscriber('/mavros/state', State,
self.process_state)
rospy.loginfo('%s: initialized into %s mode' % (self.name, self.FSM))
def _close(self):
param_set_proxy = rospy.ServiceProxy('/mavros/param/set', ParamSet)
# Unset Mavros as GCS
rospy.wait_for_service('/mavros/param/set')
try:
info = ParamSet()
info.param_id = 'SYSID_MYGCS'
val = ParamValue()
val.integer = 255
val.real = 0.0
info.value = val
param_set_proxy(info.param_id, info.value)
rospy.loginfo('Changed SYSID_MYGCS to %d', val.integer)
except rospy.ServiceException, e:
print ("/mavros/set_mode service call failed: %s"%e)
def change_param_val(self, para="None", value=0, intflag=True):
"""
Change parameter values through MavROS
"""
rospy.wait_for_service('/mavros/param/set')
try:
param_val = ParamValue()
if intflag:
param_val.integer = value
else:
param_val.real = value
ret = self._change_param(str(para), param_val)
if ret.success:
rospy.loginfo("Changed {0} to {1}".format(str(para),value))
else:
rospy.loginfo("Failed changing {0}".format(str(para)))
except rospy.ServiceException, e:
rospy.loginfo("Service call failed")
def load_param_file(px4_file):
result = True
err_lines = ""
err_params = ""
lines_commented = ""
params_loaded = ""
try:
px4_params = open(px4_file)
except IOError:
logger.error("File {} can't be opened".format(filepath))
result = False
else:
with open(px4_file) as px4_params:
row = 0
for line in px4_params:
row += 1
param_str_array = line.split('\t')
if len(param_str_array) == 5 and '#' not in param_str_array[0]:
param_name = param_str_array[2]
param_value_str = param_str_array[3]
param_type = int(param_str_array[4])
if param_type == 6:
param_value = ParamValue(integer=int(param_value_str))
else:
param_value = ParamValue(real=float(param_value_str))
if not set_param(param_name, param_value):
err_params += "{} ,".format(row)
result = False
else:
params_loaded += "{} ,".format(row)
elif '#' in param_str_array[0]:
lines_commented += "{} ,".format(row)
else:
err_lines += "{} ,".format(row)
if err_lines:
logger.info("Can't parse lines: {}".format(err_lines[:-1]))
if err_params:
logger.info("Can't set params from lines: {}".format(err_params[:-1]))
if lines_commented:
logger.info("Lines commented: {}".format(lines_commented[:-1]))
if params_loaded:
logger.info("Params are successfully loaded from lines: {}".format(
params_loaded[:-1]))
return result
def load_param_file(px4_file):
result = True
try:
px4_params = open(px4_file)
except IOError:
logging.error("File {} can't be opened".format(filepath))
result = False
else:
with open(px4_file) as px4_params:
for line in px4_params:
param_str_array = line[:-1].split('\t')
param_name = param_str_array[2]
param_value_str = param_str_array[3]
param_type = param_str_array[4]
if param_type == '6':
param_value = ParamValue(integer=int(param_value_str))
else:
param_value = ParamValue(real=float(param_value_str))
if not set_param(param_name, param_value):
result = False
return result
def set_mount_mode_parameters(self):
mode_in_value = ParamValue()
mode_out_value = ParamValue()
mode_in_value.integer = 3
mode_out_value.integer = 0
last_request = rospy.Time.now()
while not rospy.is_shutdown():
if (rospy.Time.now() - last_request) > rospy.Duration(5.0):
rospy.loginfo('Drone %d: setting up gimbal parameters' %
self.id)
try:
get_param_in_response = self.param_get_service.call(
"MNT_MODE_IN")
if get_param_in_response.success and get_param_in_response.value.integer != 3:
self.param_set_service.call("MNT_MODE_IN",
mode_in_value)
get_param_out_response = self.param_get_service.call(
"MNT_MODE_OUT")
if get_param_out_response.success and get_param_out_response.value.integer != 0:
self.param_set_service.call("MNT_MODE_OUT",
mode_out_value)
if get_param_in_response.value.integer == 3 and get_param_out_response.value.integer == 0:
break
except rospy.ServiceException, e:
print "\nService call failed: %s" % e
last_request = rospy.Time.now()
time.sleep(0.2)
def set_param(self, param, value):
"""Set parameter value for PX4 configuration"""
param_value = ParamValue()
if isinstance(value, int):
param_value.integer = value
elif isinstance(value, float):
param_value.real = value
else:
rospy.logerr("Set param value must be an integer or a real number")
return False
try:
res = self.set_param_proxy(param, param_value)
except rospy.ServiceException:
rospy.logerr("Failed to call set param service")
return False
else:
if res.success:
rospy.loginfo("{0} param set to {1}".format(param, value))
return True
else:
rospy.logerr("Response from set param service failed")
return False
def Param_Set(height, auto_velocity):
#rospy.wait_for_service('/mavros/param/set')
try:
Parameter_Set = rospy.ServiceProxy('/mavros/param/set', mavros_msgs.srv.ParamSet)
mySpeed = ParamValue()
mySpeed.real = auto_velocity
Parameter_Set('WPNAV_SPEED', mySpeed)
myRtlAlt = ParamValue()
myRtlAlt.real = height*100
Parameter_Set('RTL_ALT', myRtlAlt)
except rospy.ServiceException, e:
print "Service call failed: %s", e
def run(self):
""" Keep nodes alive until exit conditions present """
rate = rospy.Rate(self._run_rate)
while not rospy.is_shutdown(
) and self.mavros_interface._node_alive and self.node_alive:
# first check which items need to be changed
self.incorrect_params_dict = {}
for param, val in self.params_dict.items():
# rospy.loginfo((param, val))
param_read_attempts = 0
while param_read_attempts < 5:
res = self.mavros_interface.get_param_srv(param)
print(res)
if res.success:
# todo - if res == false then: try again? or just add to list
eeprom_val = res.value.integer
if val != eeprom_val:
self.incorrect_params_dict[param] = ParamValue(
val, val)
rospy.loginfo(
'param {} is currently set to {} not {} - adding to dictionary'
.format(param, eeprom_val, val))
else:
rospy.loginfo(
'param {} is already set to {} not adding to dictionary'
.format(param, val))
break
else:
rospy.logwarn(
"Couldn't read param {} on attempt {} - trying again"
.format(param, param_read_attempts))
rospy.sleep(1)
param_read_attempts += 1
# if no items need to be changed then exit loop
if len(self.incorrect_params_dict) == 0:
rospy.loginfo('All params correct - finishing up')
break
else:
for param, val in self.incorrect_params_dict.items():
rospy.loginfo(('attempting to change {} to {}'.format(
param, val.integer)))
param_write_attempts = 0
while param_write_attempts < 5:
res = self.mavros_interface.set_param_srv(param, val)
if res.success:
rospy.loginfo('result for {} is: {}'.format(
param, res.success))
break
else:
rospy.logerr(
"Couldn't read param {} on attempt {} - trying again"
.format(param, param_write_attempts))
rospy.sleep(1)
param_write_attempts += 1
rate.sleep()
rospy.loginfo('Finished setting params')
def __init__(self):
smach.State.__init__(self, outcomes=['succeeded'])
self.msg_param = ParamValue(integer=0, real=10.9)
self.param_srv = rospy.ServiceProxy("/mavros/param/set", ParamSet)
def set_xy_speed(self,speed):
rospy.wait_for_service('/mavros/param/set')
speed_service = rospy.ServiceProxy('/mavros/param/set',ParamSet)
speed_service('MPC_XY_VEL_MAX',ParamValue(0,speed))
def cbPubCmd(self, event):
if not self.started: return
if self.home_pos[2] < 1: return # Wait for home pos to be received
# Check if distance to waypoint is small enough to count it as reached TODO may be wrong approximation and TODO change for RTK
if self.getDistanceBetweenGPS(self.current_pos,
self.waypoints[self.currentWPidx]) < 2:
self.currentWPidx += 1
if self.currentWPidx == self.maxWPidx:
rospy.loginfo("MISSION DONE")
self.started = False
return
# Log info about current waypoint
rospy.loginfo("Current waypoint: " + str(self.currentWPidx) + " / " +
str(self.maxWPidx))
# Check if mode needs to be changed for OFFBOARD and ARM vehicle (this is a startup procedure)
if str(self.current_state.mode) != "OFFBOARD" and rospy.Time.now(
) - self.last_request > rospy.Duration(5.0):
resp = self.set_mode_client(0, "OFFBOARD")
if resp.mode_sent:
rospy.loginfo("Offboard enabled")
self.last_request = rospy.Time.now()
else:
if not self.current_state.armed and rospy.Time.now(
) - self.last_request > rospy.Duration(5.0):
resp = self.arming_client(True)
if resp.success:
rospy.loginfo("Vehicle armed")
self.last_request = rospy.Time.now()
# Publish information - where should the drone fly next?
pose = PoseStamped()
latWP = self.waypoints[self.currentWPidx][0]
lonWP = self.waypoints[self.currentWPidx][1]
altWP = self.waypoints[self.currentWPidx][2]
velWP = self.waypoints[self.currentWPidx][3]
# latHome = self.home_pos[0]
# lonHome = self.home_pos[1]
# altHome = self.home_pos[2]
# pose.pose.position.x = 6371000 * radians(lonWP - lonHome) * cos(radians(latHome))
# pose.pose.position.y = 6371000 * radians(latWP - latHome)
# pose.pose.position.z = altWP - altHome
#self.local_pos_pub.publish(pose)
if self.ready:
msg = GlobalPositionTarget()
msg.latitude = latWP
msg.longitude = lonWP
msg.altitude = altWP
msg.header.stamp = rospy.Time.now()
msg.coordinate_frame = 5
msg.type_mask = 0b101111111000
msg.yaw = self.getNextYaw()
self.global_pos_pub.publish(msg)
par = ParamValue()
par.integer = 0
par.real = velWP
try:
self.set_vel("MPC_XY_VEL_MAX", par)
except Exception:
print("e")
else:
pose = PoseStamped()
pose.pose.position.x = 0
pose.pose.position.y = 0
pose.pose.position.z = 2.0
self.local_pos_pub.publish(pose)
try:
par = ParamValue()
par.integer = 0
par.real = velWP
resp = self.set_vel("MPC_XY_VEL_MAX", par)
if resp.success: self.ready = True
except Exception as e:
print(e)
