Port kinematics_base submodule moveit_core

[vmayoral]

No description provided.

[vmayoral]

@mlautman and @davetcoleman please have a look again at this PR.

I've opted for not using LOGGER for now since as is, it'll need to be defined in the header causing conflicts with other submodules. Alternatively, we can change a bit the distribution of the code and push the implementation of searchPositionIK and other functions (now implemented in the header file) to the cpp file. Let me know if this approach is acceptable and I'll make the changes.

[vmayoral]

@davetcoleman and @mlautman, consider again this PR

[henningkayser]

auto is nice where the type is obvious but in cases like this it's more confusing then helpful.
Especially since the following for loops are using auto as well this type should be made explicit.

[henningkayser]

Also, the SyncParametersClient class contains the functions has_param() and get_parameter() which can be used to replace hasParam() and param() directly. This way we don't have to loop and compare the results as well.

[vmayoral]

Partially complied with your request @henningkayser at b2d436f.

Please make a suggestion if you require further changes.

[henningkayser]

get_parameters() is meant to be used for multiple parameters but here it's always called with only a single entry. In this case it's much more straightforward to query every parameter like in the sample code below:

    bool param_success = node.get_parameter<T>(group_name + "/" + param, val);

No need to create a parameter vector or loop over the results and also we can use the node directly and don't need to cast it to rclcpp::SyncParametersClient.

Since lookupParam() does in fact query multiple parameters in different locations we can absolutely use Node::get_parameters() but in that case including all parameter names:

val = default_val;
std::vector<std::string> param_names = {
  group_name_ + "/" + param, param,
  "robot_description_kinematics/" + group_name_ + "/" + param,
  robot_description_kinematics/" + param
};
std::vector<rclcpp::parameter::ParameterVariant> param_results =
  node.getParameters(param_names);
bool param_success = !param_results.empty();
if (param_success)
    val = param_results[0].get_value<T>();
return param_success;

I actually prefer the second implementation.
Please also note that your current implementation calls value_to_string() on all parameters but lookupParam() is a template function that supports all parameter types. The get_value() function takes care of using the correct type.

[anasarrak]

I've already made some changes (on planning_scene_monitor) related to this:

https://github.com/AcutronicRobotics/moveit2/blob/5a8ad535e33548434b02e5e9e735ab614465f5a5/moveit_ros/planning/planning_scene_monitor/src/planning_scene_monitor.cpp#L1472-L1516

[henningkayser]

@anasarrak this looks much better. However, each if/else block can be implemented in a single line using node->get_parameter_or(param, val, default_val).

[henningkayser]

Please advice if we're good to go here

To me it looks like #8 (comment) wasn't really addressed. The new implementation of lookupParam() is overly complicated and should be implemented following either one of the approaches discussed in the comments.

[henningkayser]

@vmayoral I took the freedom and reimplemented the lookupParam() function as I requested and also resolved the merge conflicts. I think this is ready to be merged now.

[mlautman]

This is a vector. I think you need to get the parameter at first index right? @henningkayser

[mlautman]

nit picky but I think this should be the order:

 { 
    param,
    group_name_ + "/" + param,  
    "robot_description_kinematics/" + param,
    "robot_description_kinematics/" + group_name_ + "/" + param
}

[henningkayser]

This is a vector. I think you need to get the parameter at first index right? @henningkayser

@mlautman You're absolutely right. Fixed!

nit picky but I think this should be the order:

 { 
    param,
    group_name_ + "/" + param,  
    "robot_description_kinematics/" + param,
    "robot_description_kinematics/" + group_name_ + "/" + param
}

I kept the old order and don't think we should change it.
After all the specification (with group name) should override the default (without group name), no?

[mlautman]

fair enough. My nit was aesthetic

stale

[mlautman]

@henningkayser lgtm