Hi, I am trying to set a MAVROS parameter in python but something does not work. Getting the parameter works.
Does anyone have a working example?
If I do: rosrun mavros mavparam dump - I can see the parameter I am interested in.
For getting the parameter via MAVROS in python I do the following:
import rospy
import mavros
from mavros_msgs.msg import OverrideRCIn, ParamValue, AttitudeTarget
from mavros_msgs.srv import SetMode, CommandBool, ParamPull, ParamPush, ParamGet, ParamSet
REQUIRED_PARAMETERS = [
"/topic/roll",
"/topic/pitch",
"/topic/yaw",
"/topic/hover_constrained",
"/topic/hover_unconstrained",
"/service/action_info",
"/service/action_cmd",
"/step_controller/is_constrained",
"/step_controller/step_time",
"/step_controller/thrust",
"/step_controller/step_angles/roll",
"/step_controller/step_angles/pitch",
"/step_controller/step_angles/yaw",
"/step_controller/hover_pose/x",
"/step_controller/hover_pose/y",
"/step_controller/hover_pose/z",
"/step_controller/hover_pose/yaw",
"/step_controller/tune_attitude_rate",
"step_controller/attitude_rate_step_command"
]
class StepController:
def __init__(self):
# Check existence of needed parameters
for p in REQUIRED_PARAMETERS:
if not rospy.has_param(p):
rospy.logerr("[STEP CONTROLLER] Parameter {} not found on parameter server".format(p))
# set up services to switch modes
set_mode_topic = 'mavros/set_mode'
set_arming_topic = 'mavros/cmd/arming'
set_param_topic = 'mavros/param/set'
get_param_topic = 'mavros/param/get'
push_param_topic = 'mavros/param/push'
self.set_mode_srv = rospy.ServiceProxy(set_mode_topic, SetMode)
self.set_arming_srv = rospy.ServiceProxy(set_arming_topic, CommandBool)
self.set_param_srv = rospy.ServiceProxy(set_param_topic, ParamSet)
self.get_param_srv = rospy.ServiceProxy(get_param_topic, ParamGet)
self.push_param_srv = rospy.ServiceProxy(push_param_topic, ParamPush)
# Get parameters
self.roll_topic = rospy.get_param("/topic/roll")
self.pitch_topic = rospy.get_param("/topic/pitch")
self.yaw_topic = rospy.get_param("/topic/yaw")
self.hover_constrained_topic = rospy.get_param("/topic/hover_constrained")
self.hover_unconstrained_topic = rospy.get_param("/topic/hover_unconstrained")
self.action_info_service = rospy.get_param("/service/action_info")
self.action_cmd_service = rospy.get_param("/service/action_cmd")
def set_param(self, param_name, value, is_int=False):
try:
rospy.set_param('/mavros/param/'+param_name, float(value))
answ = self.push_param_srv()
except rospy.ServiceException as ex:
rospy.logwarn("Param "+param_name+" not successfully set")
try:
rospy.set_param('/mavros/param/'+param_name, int(value))
answ = self.push_param_srv()
except:
print()
def get_param(self, param_name):
answ = self.get_param_srv(param_name)
if answ.success:
if answ.value.integer != 0:
value = answ.value.integer
# rospy.loginfo("Parameter "+param_name+" read. Value: "+str(answ.value.integer))
else:
value = answ.value.real
# rospy.loginfo("Parameter "+param_name+" read. Value: "+str(answ.value.real))
else:
rospy.logwarn("Parameter "+param_name+" not read")
value = 0
return value
if __name__ == '__main__':
try:
rospy.init_node("step_controller_node")
rospy.loginfo("[STEP CONTROLLER] Ready")
step_controller = StepController()
# the setpoint publishing rate MUST be faster than 2Hz
rate = rospy.Rate(20.0) # 20hz
last_request = rospy.get_rostime()
service_timeout = 20
# Wait for the param set/get services
print("wait...")
rospy.wait_for_service('/mavros/param/set', service_timeout)
rospy.wait_for_service('mavros/param/get', service_timeout)
print("finish wait...")
# Try to set the param
rospy.sleep(2)
step_controller.set_param('FD_FAIL_R', 333.0)
while not rospy.is_shutdown():
# Wait for the param set/get services
rospy.logwarn("wait...")
rospy.wait_for_service('/mavros/param/set', service_timeout)
rospy.wait_for_service('mavros/param/get', service_timeout)
rospy.logwarn("finish wait...")
# Try to get the param
rospy.sleep(2)
param = step_controller.get_param('FD_FAIL_R')
rospy.logwarn("FD_FAIL_R: "+str(param), )
# Try to set the parameter
step_controller.set_param('FD_FAIL_R', 333.0)
except rospy.ROSInterruptException:
pass
I get the following output:
>> [ WARN] [1604679262.041814537, 11.396000000]: PR: Resend param set for FD_FAIL_R, retries left 2
>> [ WARN] [1604679263.079001196, 12.396000000]: PR: Resend param set for FD_FAIL_R, retries left 1
>> [ WARN] [1604679264.141189118, 13.396000000]: PR: Resend param set for FD_FAIL_R, retries left 0
>> [ERROR] [1604679265.183162810, 14.396000000]: PR: Param set for FD_FAIL_R timed out.
>> [WARN] [1604679265.189511, 14.400000]: full_param_name:None
>> [WARN] [1604679265.226551, 14.436000]: wait...
>> [WARN] [1604679265.254289, 14.460000]: finish wait...
>> [WARN] [1604679267.642134, 16.472000]: FD_FAIL_R: 365
>> [ WARN] [1604679268.832823429, 17.492000000]: PR: Resend param set for FD_FAIL_R, retries left 2
>> [ WARN] [1604679269.912936794, 18.492000000]: PR: Resend param set for FD_FAIL_R, retries left 1
>> [ WARN] [1604679271.055157806, 19.492000000]: PR: Resend param set for FD_FAIL_R, retries left 0
>> [ERROR] [1604679272.117609843, 20.496000000]: PR: Param set for FD_FAIL_R timed out.