Merge branch 'fix/pywalk' into feature/mujoco

Author: Flova

bitbots_motion/bitbots_quintic_walk/bitbots_quintic_walk_py/py_walk.py

@@ -1,3 +1,5 @@
+from typing import Optional
+
 from biped_interfaces.msg import Phase
 from bitbots_quintic_walk_py.libpy_quintic_walk import PyWalkWrapper
 from bitbots_utils.utils import parse_parameter_dict
@@ -11,17 +13,24 @@
 
 
 class PyWalk:
-    def __init__(self, namespace="", parameters: list[Parameter] | None = None, set_force_smooth_step_transition=False):
-        serialized_parameters = []
-        if parameters is not None:
-            for parameter in parameters:
-                serialized_parameters.append(serialize_message(parameter))
-                if parameter.value.type == 2:
-                    print(
-                        f"Gave parameter {parameter.name} of integer type. If the code crashes it is maybe because this "
-                        f"should be a float. You may need to add an .0 in some yaml file."
-                    )
-        self.py_walk_wrapper = PyWalkWrapper(namespace, serialized_parameters, set_force_smooth_step_transition)
+    def __init__(
+        self,
+        namespace="",
+        walk_parameters: Optional[list[Parameter]] = None,
+        moveit_parameters: Optional[list[Parameter]] = None,
+        set_force_smooth_step_transition=False,
+    ):
+        def serialize_parameters(parameters):
+            if parameters is None:
+                return []
+            return list(map(serialize_message, parameters))
+
+        self.py_walk_wrapper = PyWalkWrapper(
+            namespace,
+            serialize_parameters(walk_parameters),
+            serialize_parameters(moveit_parameters),
+            set_force_smooth_step_transition,
+        )
 
     def spin_ros(self):
         self.py_walk_wrapper.spin_some()

bitbots_motion/bitbots_quintic_walk/include/bitbots_quintic_walk/walk_node.hpp

@@ -54,7 +54,7 @@ namespace bitbots_quintic_walk {
 class WalkNode {
  public:
   explicit WalkNode(rclcpp::Node::SharedPtr node, const std::string &ns = "",
-                    std::vector<rclcpp::Parameter> parameters = {});
+                    std::vector<rclcpp::Parameter> moveit_parameters = {});
   bitbots_msgs::msg::JointCommand step(double dt);
   bitbots_msgs::msg::JointCommand step(double dt, geometry_msgs::msg::Twist::SharedPtr cmdvel_msg,
                                        sensor_msgs::msg::Imu::SharedPtr imu_msg,
@@ -112,8 +112,6 @@ class WalkNode {
 
   nav_msgs::msg::Odometry getOdometry();
 
-  rcl_interfaces::msg::SetParametersResult onSetParameters(const std::vector<rclcpp::Parameter> &parameters);
-
   void publish_debug();
   rclcpp::TimerBase::SharedPtr startTimer();
   double getTimerFreq();

bitbots_motion/bitbots_quintic_walk/include/bitbots_quintic_walk/walk_pywrapper.hpp

@@ -23,8 +23,8 @@ using namespace ros2_python_extension;
 
 class PyWalkWrapper {
  public:
-  explicit PyWalkWrapper(std::string ns, std::vector<py::bytes> parameter_msgs = {},
-                         bool force_smooth_step_transition = false);
+  explicit PyWalkWrapper(std::string ns, std::vector<py::bytes> walk_parameter_msgs = {},
+                         std::vector<py::bytes> moveit_parameter_msgs = {}, bool force_smooth_step_transition = false);
   py::bytes step(double dt, py::bytes &cmdvel_msg, py::bytes &imu_msg, py::bytes &jointstate_msg,
                  py::bytes &pressure_left, py::bytes &pressure_right);
   py::bytes step_relative(double dt, py::bytes &step_msg, py::bytes &imu_msg, py::bytes &jointstate_msg,

bitbots_motion/bitbots_quintic_walk/src/walk_node.cpp

@@ -9,29 +9,23 @@ using namespace std::chrono_literals;
 
 namespace bitbots_quintic_walk {
 
-WalkNode::WalkNode(rclcpp::Node::SharedPtr node, const std::string& ns, std::vector<rclcpp::Parameter> parameters)
+WalkNode::WalkNode(rclcpp::Node::SharedPtr node, const std::string& ns,
+                   std::vector<rclcpp::Parameter> moveit_parameters)
     : node_(node),
       param_listener_(node_),
       config_(param_listener_.get_params()),
       walk_engine_(node_, config_.engine),
       stabilizer_(node_),
       ik_(node_, config_.node.ik),
       visualizer_(node_, config_.node.tf) {
-  // Create dummy node for moveit
-  auto moveit_node = std::make_shared<rclcpp::Node>(ns + "walking_moveit_node");
-
-  // when called from python, parameters are given to the constructor
-  for (auto parameter : parameters) {
-    if (node_->has_parameter(parameter.get_name())) {
-      // this is the case for walk engine params set via python
-      node_->set_parameter(parameter);
-    } else {
-      // parameter is not for the walking, set on moveit node
-      moveit_node->declare_parameter(parameter.get_name(), parameter.get_type());
-      moveit_node->set_parameter(parameter);
-    }
-  }
-
+  // Create dummy node for moveit. This is necessary for dynamic reconfigure to work (moveit does some bullshit with
+  // parameter declarations, so we need to isolate the walking parameters from the moveit parameters).
+  // If the walking is instantiated using the python wrapper, moveit parameters are passed because no moveit config
+  // is loaded in the conventional way. Normally the moveit config is loaded via launch file and the passed vector is
+  // empty.
+  auto moveit_node = std::make_shared<rclcpp::Node>(
+      "walking_moveit_node", ns,
+      rclcpp::NodeOptions().automatically_declare_parameters_from_overrides(true).parameter_overrides(moveit_parameters));
   // get all kinematics parameters from the move_group node if they are not set manually via constructor
   std::string check_kinematic_parameters;
   if (!moveit_node->get_parameter("robot_description_kinematics.LeftLeg.kinematics_solver",

bitbots_motion/bitbots_quintic_walk/src/walk_pywrapper.cpp

@@ -2,21 +2,37 @@
 
 void PyWalkWrapper::spin_some() { rclcpp::spin_some(node_); }
 
-PyWalkWrapper::PyWalkWrapper(std::string ns, std::vector<py::bytes> parameter_msgs, bool force_smooth_step_transition) {
+PyWalkWrapper::PyWalkWrapper(std::string ns, std::vector<py::bytes> walk_parameter_msgs,
+                             std::vector<py::bytes> moveit_parameter_msgs, bool force_smooth_step_transition) {
   // initialize rclcpp if not already done
   if (!rclcpp::contexts::get_global_default_context()->is_valid()) {
     rclcpp::init(0, nullptr);
   }
 
-  // create parameters from serialized messages
-  std::vector<rclcpp::Parameter> cpp_parameters = {};
-  for (auto &parameter_msg : parameter_msgs) {
-    cpp_parameters.push_back(
-        rclcpp::Parameter::from_parameter_msg(fromPython<rcl_interfaces::msg::Parameter>(parameter_msg)));
-  }
-
-  node_ = rclcpp::Node::make_shared(ns + "walking");
-  walk_node_ = std::make_shared<bitbots_quintic_walk::WalkNode>(node_, ns, cpp_parameters);
+  // internal function to deserialize the parameter messages
+  auto deserialize_parameters = [](std::vector<py::bytes> parameter_msgs) {
+    std::vector<rclcpp::Parameter> cpp_parameters = {};
+    for (auto &parameter_msg : parameter_msgs) {
+      cpp_parameters.push_back(
+          rclcpp::Parameter::from_parameter_msg(fromPython<rcl_interfaces::msg::Parameter>(parameter_msg)));
+    }
+    return cpp_parameters;
+  };
+
+  // Create a node object
+  // Even tho we use python bindings instead of ros's dds, we still need a node object for logging and parameter
+  // handling Because the walking is not started using the launch infrastructure and an appropriate parameter file, we
+  // need to manually set the parameters
+  node_ = rclcpp::Node::make_shared(
+      "walking", ns, rclcpp::NodeOptions().parameter_overrides(deserialize_parameters(walk_parameter_msgs)));
+
+  // Create the walking object
+  // We pass it the node we created. But the walking also creates a helper node for moveit (otherwise dynamic
+  // reconfigure does not work, because moveit does some bullshit with their parameter declarations leading dynamic
+  // reconfigure not working). This way the walking parameters are isolated from the moveit parameters, allowing dynamic
+  // reconfigure to work. Therefore we need to pass the moveit parameters to the walking.
+  walk_node_ =
+      std::make_shared<bitbots_quintic_walk::WalkNode>(node_, ns, deserialize_parameters(moveit_parameter_msgs));
   set_robot_state(0);
   walk_node_->initializeEngine();
   walk_node_->getEngine()->setForceSmoothStepTransition(force_smooth_step_transition);
@@ -197,7 +213,7 @@ PYBIND11_MODULE(libpy_quintic_walk, m) {
   using namespace bitbots_quintic_walk;
 
   py::class_<PyWalkWrapper, std::shared_ptr<PyWalkWrapper>>(m, "PyWalkWrapper")
-      .def(py::init<std::string, std::vector<py::bytes>, bool>())
+      .def(py::init<std::string, std::vector<py::bytes>, std::vector<py::bytes>, bool>())
       .def("step", &PyWalkWrapper::step)
       .def("step_relative", &PyWalkWrapper::step_relative)
       .def("step_open_loop", &PyWalkWrapper::step_open_loop)