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