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)