Closed Legohead259 closed 7 months ago
pablogs9
commented
8 months ago Could you check if a simpler example like this one: https://github.com/micro-ROS/micro_ros_espidf_component/blob/iron/examples/parameters/main/main.c works properly on your platform?
Legohead259
commented
7 months ago Found the error! I am using a lookup array to keep track of all the parameter objects. In my implementation, I want each parameter to trigger a callback that affects the electronic speed controller part of the code. For example: changing the controller mode parameter triggers a callback that performs some controller_mode-specific logic for the change. Same with the other parameters. To that end, the parameter change callback for the parameter server used to look like this:
bool onParameterChangedCallback(const Parameter* oldParam, const Parameter* newParam, void* context) {
(void) context;
if (oldParam == NULL && newParam == NULL) { // Check for callback error
Serial1.printf("Callback error, both parameters are NULL\n"); // Debug
return false;
}
if (oldParam == NULL) { // Check for new parameter
Serial1.printf("Creating new parameter %s\n", newParam->name.data); // Debug
return false; // Do not allow new parameters
}
else if (newParam == NULL) { // Check for deleting parameter
Serial1.printf("Deleting parameter %s\n", oldParam->name.data); // Debug
return false; // Do not allow deleting parameters
}
else { // Check for changing parameters
Serial1.printf("Checking for: %s\r\n", newParam->name.data);
for (uint8_t i=0; i<NUM_PARAMETERS; i++) { // Iterate through the parameter array
Serial1.printf("\t Comparing against: %s\r\n", params[i]->key);
if (strcmp(newParam->name.data, params[i]->key) == 0) { // Check if parameter is in array
Serial1.printf("Editing %s\r\n", params[i]->key); // DEBUG
switch (newParam->value.type) { // Check parameter type and set new value accordingly
case RCLC_PARAMETER_BOOL:
params[i]->bool_value = newParam->value.bool_value;
break;
case RCLC_PARAMETER_INT:
params[i]->integer_value = newParam->value.integer_value;
break;
case RCLC_PARAMETER_DOUBLE:
params[i]->double_value = newParam->value.double_value;
break;
default:
break;
}
params[i]->onChange(); // Execute the parameters change function
saveParam(params[i]);
break;
}
else { return false; }
}
}
return true;
}The logic within the for-loop that checks the newParam.name.data against the key of each param in the array only executes once. If the first key in the parameter array does not match the name supplied by newParam.name.data, then it automatically returns false, which causes the parameter server to throw a hissy-fit. Not to mention, that when I initialize all the parameter values at the start of the micro-ros task, everything except the first parameter in the parameter array is not initialized - hence my issue.
To fix the issue, just had to adjust the exit case logic:
[...]
else { // Check for changing parameters
Serial1.printf("Checking for: %s\r\n", newParam->name.data);
for (uint8_t i=0; i<NUM_PARAMETERS; i++) { // Iterate through the parameter array
Serial1.printf("\t Comparing against: %s\r\n", params[i]->key);
if (strcmp(newParam->name.data, params[i]->key) == 0) { // Check if parameter is in array
Serial1.printf("Editing %s\r\n", params[i]->key); // DEBUG
switch (newParam->value.type) { // Check parameter type and set new value accordingly
case RCLC_PARAMETER_BOOL:
params[i]->bool_value = newParam->value.bool_value;
break;
case RCLC_PARAMETER_INT:
params[i]->integer_value = newParam->value.integer_value;
break;
case RCLC_PARAMETER_DOUBLE:
params[i]->double_value = newParam->value.double_value;
break;
default:
break;
}
params[i]->onChange(); // Execute the parameters change function
saveParam(params[i]);
- break;
+ return true;
}
- else { return false; }
}
+ return false;
}
[...]Expected and correct output from ROS2 CLI:
~/ROS2-Micro-ROS-ESC-Workspace$ ros2 param get bldc_node angle_p
Double value is: 20.0
~/ROS2-Micro-ROS-ESC-Workspace$ ros2 param set bldc_node angle_p 21.0
Set parameter successful
Issue template
Steps to reproduce the issue
ros2 param listros2 param get /bldc_node [param name]ros2 param set /bldc_node [param_name] [param_value]ros2 param get /bldc_node [param_name]Expected behavior
Parameter List Output
**Works as expected** ```bash /bldc_node: angle_d angle_i angle_lpf_tf angle_p angle_ramp controller_mode driver_v_limit kv_rating motor_i_limit motor_v_limit motor_vel_limit phase_l phase_r pole_pairs velocity_d velocity_i velocity_lpf_tf velocity_p velocity_ramp ```Expected output from:
ros2 param get /bldc_node contoller_node:Expected output from
ros2 param set /bldc_node controller_mode 5 && ros2 param get /bldc_node controller_modeActual behavior
Actual output from:
ros2 param get /bldc_node contoller_nodeon initial connection:But, the external serial debugger reports that the controller mode is successfully set to 7 on Micro-ROS parameter server start up. This behavior repeats for every parameter. Each one reports a value of 0, despite being correctly set on the board. Additionally, the parameter server functions as expected. Changing the parameter from the host PC successfully changes the parameter value on the Micro ROS client and then the
ros2 param getcommand reports the correct value.Additional information
Here is the code where the parameter service is initialized and the parameters added. I'm pretty sure I followed the parameter server tutorial properly from the docs.
micro_ros__parameters.h
```cpp #ifndef MICRO_ROS__PARAMETERS_H #define MICRO_ROS__PARAMETERS_H #include "micro_ros__definitions.h" // ============================ // === PARAMETER DEFINTIONS === // ============================ #ifdef ARDUINO_ARCH_ESP32 extern Preferences parameterSettings; #endif // ARDUINO_ARCH_ESP32 // NOTE: Due to restrictions within the Preferences library, parameter names are limited to 16 characters! #define PARAM_NAME__CONTROLLER_MODE "controller_mode" #define PARAM_NAME__POLE_PAIRS "pole_pairs" #define PARAM_NAME__PHASE_RESISTANCE "phase_r" #define PARAM_NAME__KV_RATING "kv_rating" #define PARAM_NAME__PHASE_INDUCTANCE "phase_l" #define PARAM_NAME__VELOCITY_P "velocity_p" #define PARAM_NAME__VELOCITY_I "velocity_i" #define PARAM_NAME__VELOCITY_D "velocity_d" #define PARAM_NAME__VELOCITY_OUTPUT_RAMP "velocity_ramp" #define PARAM_NAME__VELOCITY_LPF_TF "velocity_lpf_tf" #define PARAM_NAME__ANGLE_P "angle_p" #define PARAM_NAME__ANGLE_I "angle_i" #define PARAM_NAME__ANGLE_D "angle_d" #define PARAM_NAME__ANGLE_OUTPUT_RAMP "angle_ramp" #define PARAM_NAME__ANGLE_LPF_TF "angle_lpf_tf" #define PARAM_NAME__DRIVER_VOLTAGE_LIMIT "driver_v_limit" #define PARAM_NAME__MOTOR_VOLTAGE_LIMIT "motor_v_limit" #define PARAM_NAME__MOTOR_CURRENT_LIMIT "motor_i_limit" #define PARAM_NAME__MOTOR_VELOCITY_LIMIT "motor_vel_limit" #define NUM_PARAMETERS 19 struct parameter_t; // Forward declare the struct typedef std::functionmicro_ros__parameters.cpp
```cpp #include "micro_ros__parameters.h" // ============================= // === PARAMETER DEFINITIONS === // ============================= #ifdef ARDUINO_ARCH_ESP32 Preferences parameterSettings; #endif // ARDUINO_ARCH_ESP32 rclc_parameter_server_t parameterService; parameter_t controllerMode = { .key=PARAM_NAME__CONTROLLER_MODE, .type=RCLC_PARAMETER_INT, .onChangePtr=controllerModeChangeCallback }; parameter_t polePairs{ .key=PARAM_NAME__POLE_PAIRS, .type=RCLC_PARAMETER_INT, .onChangePtr=polePairsChangeCallback }; parameter_t phaseResistance{ .key=PARAM_NAME__PHASE_RESISTANCE, .type=RCLC_PARAMETER_DOUBLE, .onChangePtr=phaseResistanceChangeCallback }; parameter_t kvRating{ .key=PARAM_NAME__KV_RATING, .type=RCLC_PARAMETER_INT, .onChangePtr=kvRatingChangeCallback }; parameter_t phaseInductance{ .key=PARAM_NAME__PHASE_INDUCTANCE, .type=RCLC_PARAMETER_DOUBLE, .onChangePtr=phaseInductanceChangeCallback }; parameter_t velocityP{ .key=PARAM_NAME__VELOCITY_P, .type=RCLC_PARAMETER_DOUBLE, .onChangePtr=velocityPChangeCallback }; parameter_t velocityI{ .key=PARAM_NAME__VELOCITY_I, .type=RCLC_PARAMETER_DOUBLE, .onChangePtr=velocityIChangeCallback }; parameter_t velocityD{ .key=PARAM_NAME__VELOCITY_D, .type=RCLC_PARAMETER_DOUBLE, .onChangePtr=velocityDChangeCallback }; parameter_t velocityOutputRamp{ .key=PARAM_NAME__VELOCITY_OUTPUT_RAMP, .type=RCLC_PARAMETER_DOUBLE, .onChangePtr=velocityRampChangeCallback }; parameter_t velocityLPFTf{ .key=PARAM_NAME__VELOCITY_LPF_TF, .type=RCLC_PARAMETER_DOUBLE, .onChangePtr=velocityLPFChangeCallback }; parameter_t angleP{ .key=PARAM_NAME__ANGLE_P, .type=RCLC_PARAMETER_DOUBLE, .onChangePtr=anglePChangeCallback }; parameter_t angleI{ .key=PARAM_NAME__ANGLE_I, .type=RCLC_PARAMETER_DOUBLE, .onChangePtr=angleIChangeCallback }; parameter_t angleD{ .key=PARAM_NAME__ANGLE_D, .type=RCLC_PARAMETER_DOUBLE, .onChangePtr=angleDChangeCallback }; parameter_t angleOutputRamp{ .key=PARAM_NAME__ANGLE_OUTPUT_RAMP, .type=RCLC_PARAMETER_DOUBLE, .onChangePtr=angleRampChangeCallback }; parameter_t angleLPFTf{ .key=PARAM_NAME__ANGLE_LPF_TF, .type=RCLC_PARAMETER_DOUBLE, .onChangePtr=angleLFPChangeCallback }; parameter_t driverVoltageLimit{ .key=PARAM_NAME__DRIVER_VOLTAGE_LIMIT, .type=RCLC_PARAMETER_DOUBLE, .onChangePtr=driverVoltageLimitChangeCallback }; parameter_t motorVoltageLimit{ .key=PARAM_NAME__MOTOR_VOLTAGE_LIMIT, .type=RCLC_PARAMETER_DOUBLE, .onChangePtr=motorVoltageLimitChangeCallback }; parameter_t motorCurrentLimit{ .key=PARAM_NAME__MOTOR_CURRENT_LIMIT, .type=RCLC_PARAMETER_DOUBLE, .onChangePtr=motorCurrentLimitChangeCallback }; parameter_t motorVelocityLimit{ .key=PARAM_NAME__MOTOR_VELOCITY_LIMIT, .type=RCLC_PARAMETER_DOUBLE, .onChangePtr=motorVelocityLimitChangeCallback }; parameter_t* params[NUM_PARAMETERS] = { &controllerMode, &polePairs, &phaseResistance, &kvRating, &phaseInductance, &velocityP, &velocityI, &velocityD, &velocityOutputRamp, &velocityLPFTf, &angleP, &angleI, &angleD, &angleOutputRamp, &angleLPFTf, &driverVoltageLimit, &motorVoltageLimit, &motorCurrentLimit, &motorVelocityLimit }; #ifdef ARDUINO_ARCH_ESP32 void loadPreferences() { parameterSettings.begin("uros_params", false); // parameterSettings.clear(); // DEBUG if (!parameterSettings.isKey(PARAM_NAME__CONTROLLER_MODE)) { // Check for first run and initialize with defaults Serial1.println("Did not find parameter file. Initializing defaults..."); parameterSettings.putInt(PARAM_NAME__CONTROLLER_MODE, VELOCITY_OPEN_LOOP); parameterSettings.putInt(PARAM_NAME__POLE_PAIRS, 7); parameterSettings.putDouble(PARAM_NAME__PHASE_RESISTANCE, 3.0); parameterSettings.putInt(PARAM_NAME__KV_RATING, 600); parameterSettings.putDouble(PARAM_NAME__PHASE_INDUCTANCE, 3.0); parameterSettings.putDouble(PARAM_NAME__VELOCITY_P, 0.5); parameterSettings.putDouble(PARAM_NAME__VELOCITY_I, 10.0); parameterSettings.putDouble(PARAM_NAME__VELOCITY_D, 0); parameterSettings.putDouble(PARAM_NAME__VELOCITY_OUTPUT_RAMP, 300); parameterSettings.putDouble(PARAM_NAME__VELOCITY_LPF_TF, 5); parameterSettings.putDouble(PARAM_NAME__ANGLE_P, 20); parameterSettings.putDouble(PARAM_NAME__ANGLE_I, 0); parameterSettings.putDouble(PARAM_NAME__ANGLE_D, 0); parameterSettings.putDouble(PARAM_NAME__ANGLE_OUTPUT_RAMP, 1e6); parameterSettings.putDouble(PARAM_NAME__ANGLE_LPF_TF, 0); parameterSettings.putDouble(PARAM_NAME__DRIVER_VOLTAGE_LIMIT, 7); parameterSettings.putDouble(PARAM_NAME__MOTOR_VOLTAGE_LIMIT, 7); parameterSettings.putDouble(PARAM_NAME__MOTOR_CURRENT_LIMIT, 0.2); parameterSettings.putDouble(PARAM_NAME__MOTOR_VELOCITY_LIMIT, 10); } Serial1.println("Found parameter file"); // Debug Serial1.println("Loading parameters into memory..."); // Debug // Iterate through the parameter array in memory and put loaded values there for (uint8_t i = 0; i < NUM_PARAMETERS; i++) { Serial1.printf("Loading key: %s...\t", params[i]->key); // Debug if (params[i]->type == RCLC_PARAMETER_BOOL) { params[i]->bool_value = parameterSettings.getBool(params[i]->key); Serial1.printf("Got boolean parameter!\tValue: %s\r\n", params[i]->bool_value ? "TRUE" : "FALSE"); // Debug } else if (params[i]->type == RCLC_PARAMETER_INT) { params[i]->integer_value = parameterSettings.getInt(params[i]->key); Serial1.printf("Got integer parameter!\tValue: %d\r\n", params[i]->integer_value); // Debug } else if (params[i]->type == RCLC_PARAMETER_DOUBLE) { params[i]->double_value = parameterSettings.getDouble(params[i]->key); Serial1.printf("Got double parameter!\tValue: %0.3f\r\n", params[i]->double_value); // Debug } else { params[i]->type = RCLC_PARAMETER_NOT_SET; } } } #endif // ARDUINO_ARCH_ESP32 void initializeParameterService() { #ifdef ARDUINO_ARCH_ESP32 loadPreferences(); #endif // ARDUINO_ARCH_ESP32 // TODO: Support out NVS systems for other MCUs // Add parameters to the server Serial1.println("Adding parameters to server..."); for (uint8_t i = 0; i < NUM_PARAMETERS; i++) { // Iterate through array of parameter structures if (params[i]->type == RCLC_PARAMETER_NOT_SET) { continue; } // Check for bad parameters RCSOFTCHECK(rclc_add_parameter(¶meterService, params[i]->key, params[i]->type)); switch (params[i]->type) { case RCLC_PARAMETER_BOOL: RCSOFTCHECK(rclc_parameter_set_bool( ¶meterService, params[i]->key, params[i]->bool_value)); break; case RCLC_PARAMETER_INT: RCSOFTCHECK(rclc_parameter_set_int( ¶meterService, params[i]->key, params[i]->integer_value)); break; case RCLC_PARAMETER_DOUBLE: RCSOFTCHECK(rclc_parameter_set_double( ¶meterService, params[i]->key, params[i]->double_value)); break; } } } bool onParameterChangedCallback(const Parameter* oldParam, const Parameter* newParam, void* context) { (void) context; if (oldParam == NULL && newParam == NULL) { // Check for callback error Serial1.printf("Callback error, both parameters are NULL\n"); // Debug return false; } if (oldParam == NULL) { // Check for new parameter Serial1.printf("Creating new parameter %s\n", newParam->name.data); // Debug return false; // Do not allow new parameters } else if (newParam == NULL) { // Check for deleting parameter Serial1.printf("Deleting parameter %s\n", oldParam->name.data); // Debug return false; // Do not allow deleting parameters } else { // Check for changing parameters for (uint8_t i=0; i