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)