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