Feature/capture steps

bitbots_odometry/CMakeLists.txt

@@ -6,20 +6,26 @@ if (NOT CMAKE_CXX_STANDARD)
     set(CMAKE_CXX_STANDARD 17)
 endif ()
 
-find_package(tf2_eigen REQUIRED)
-find_package(tf2 REQUIRED)
-find_package(nav_msgs REQUIRED)
-find_package(message_filters REQUIRED)
-find_package(tf2_geometry_msgs REQUIRED)
+find_package(Eigen3 REQUIRED)
+find_package(ament_cmake REQUIRED)
 find_package(biped_interfaces REQUIRED)
-find_package(geometry_msgs REQUIRED)
 find_package(bitbots_docs REQUIRED)
+find_package(generate_parameter_library REQUIRED)
+find_package(geometry_msgs REQUIRED)
+find_package(message_filters REQUIRED)
+find_package(nav_msgs REQUIRED)
+find_package(rclcpp REQUIRED)
 find_package(rot_conv REQUIRED)
 find_package(sensor_msgs REQUIRED)
+find_package(tf2 REQUIRED)
+find_package(tf2_eigen REQUIRED)
+find_package(tf2_geometry_msgs REQUIRED)
 find_package(tf2_ros REQUIRED)
-find_package(ament_cmake REQUIRED)
-find_package(rclcpp REQUIRED)
-find_package(Eigen3 REQUIRED)
+
+generate_parameter_library(
+  odometry_parameters
+  config/odometry_config_template.yaml
+)
 
 include_directories(include)
 
@@ -28,37 +34,48 @@ add_compile_options(-Wall -Werror -Wno-unused)
 add_executable(odometry_fuser src/odometry_fuser.cpp)
 add_executable(motion_odometry src/motion_odometry.cpp)
 
+target_link_libraries(
+  motion_odometry
+  rclcpp::rclcpp
+  odometry_parameters
+)
+
 ## Specify libraries to link a library or executable target against
-ament_target_dependencies(motion_odometry
-        tf2_eigen
-        tf2
-        nav_msgs
-        message_filters
-        tf2_geometry_msgs
-        biped_interfaces
-        geometry_msgs
-        bitbots_docs
-        rot_conv
-        sensor_msgs
-        tf2_ros
-        ament_cmake
-        rclcpp)
+ament_target_dependencies(
+  motion_odometry
+  tf2_eigen
+  tf2
+  nav_msgs
+  message_filters
+  tf2_geometry_msgs
+  biped_interfaces
+  geometry_msgs
+  bitbots_docs
+  rot_conv
+  sensor_msgs
+  tf2_ros
+  ament_cmake
+  generate_parameter_library
+  rclcpp
+)
 
-ament_target_dependencies(odometry_fuser
-        tf2_eigen
-        tf2
-        nav_msgs
-        message_filters
-        tf2_geometry_msgs
-        biped_interfaces
-        geometry_msgs
-        bitbots_docs
-        rot_conv
-        sensor_msgs
-        tf2_ros
-        ament_cmake
-        rclcpp
-        Eigen3)
+ament_target_dependencies(
+  odometry_fuser
+  tf2_eigen
+  tf2
+  nav_msgs
+  message_filters
+  tf2_geometry_msgs
+  biped_interfaces
+  geometry_msgs
+  bitbots_docs
+  rot_conv
+  sensor_msgs
+  tf2_ros
+  ament_cmake
+  rclcpp
+  Eigen3
+)
 
 enable_bitbots_docs()
 
@@ -71,5 +88,4 @@ install(TARGETS odometry_fuser
 install(DIRECTORY launch
         DESTINATION share/${PROJECT_NAME})
 
-
 ament_package()

bitbots_odometry/config/odometry_config_amy.yaml

@@ -0,0 +1,7 @@
+motion_odometry:
+  ros__parameters:
+    x_forward_scaling: 1.25
+    x_backward_scaling: 0.95
+    y_scaling: 1.0
+    yaw_scaling: 0.6
+    publish_walk_odom_tf: false

bitbots_odometry/config/odometry_config_donna.yaml

@@ -0,0 +1,7 @@
+motion_odometry:
+  ros__parameters:
+    x_forward_scaling: 1.25
+    x_backward_scaling: 0.95
+    y_scaling: 1.0
+    yaw_scaling: 0.6
+    publish_walk_odom_tf: false

bitbots_odometry/config/odometry_config_jack.yaml

@@ -0,0 +1,7 @@
+motion_odometry:
+  ros__parameters:
+    x_forward_scaling: 1.25
+    x_backward_scaling: 0.95
+    y_scaling: 1.0
+    yaw_scaling: 0.6
+    publish_walk_odom_tf: false

bitbots_odometry/config/odometry_config_melody.yaml

@@ -0,0 +1,7 @@
+motion_odometry:
+  ros__parameters:
+    x_forward_scaling: 1.25
+    x_backward_scaling: 0.95
+    y_scaling: 1.0
+    yaw_scaling: 0.6
+    publish_walk_odom_tf: false

bitbots_odometry/config/odometry_config_rory.yaml

@@ -0,0 +1,7 @@
+motion_odometry:
+  ros__parameters:
+    x_forward_scaling: 1.25
+    x_backward_scaling: 0.95
+    y_scaling: 1.0
+    yaw_scaling: 0.6
+    publish_walk_odom_tf: false

bitbots_odometry/config/odometry_config_template.yaml

@@ -0,0 +1,42 @@
+motion_odometry:
+  x_forward_scaling: {
+    type: double,
+    default_value: 1.25,
+    description: "Scaling factor in forward x direction",
+    validation: {
+      bounds<>: [0.5, 1.5]
+    }
+  }
+
+  x_backward_scaling: {
+    type: double,
+    default_value: 0.95,
+    description: "Scaling factor in backward x direction",
+    validation: {
+      bounds<>: [0.5, 1.5]
+    }
+  }
+
+  y_scaling: {
+    type: double,
+    default_value: 1.0,
+    description: "Scaling factor in y direction",
+    validation: {
+      bounds<>: [0.5, 1.5]
+    }
+  }
+
+  yaw_scaling: {
+    type: double,
+    default_value: 0.6,
+    description: "Scaling factor for rotation in yaw direction",
+    validation: {
+      bounds<>: [0.2, 2.5]
+    }
+  }
+
+  publish_walk_odom_tf: {
+    type: bool,
+    default_value: false,
+    description:  "Should odom tf be published"
+  }

bitbots_odometry/include/bitbots_odometry/motion_odometry.h

@@ -10,9 +10,12 @@
 #include <unistd.h>
 #include <tf2_ros/buffer.h>
 #include <rclcpp/experimental/executors/events_executor/events_executor.hpp>
+#include "odometry_parameters.hpp"
 
 using std::placeholders::_1;
 
+namespace bitbots_odometry {
+
 class MotionOdometry : public rclcpp::Node {
  public:
   MotionOdometry();
@@ -33,11 +36,10 @@ class MotionOdometry : public rclcpp::Node {
   rclcpp::Subscription<sensor_msgs::msg::JointState>::SharedPtr joint_state_sub_;
   rclcpp::Subscription<nav_msgs::msg::Odometry>::SharedPtr odom_subscriber_;
 
-  double x_forward_scaling_;
-  double x_backward_scaling_;
-  double y_scaling_;
-  double yaw_scaling_;
-  bool publish_walk_odom_tf_;
+  // Declare parameter listener and struct from the generate_parameter_library
+  motion_odometry::ParamListener param_listener_;
+  // Datastructure to hold all parameters, which is build from the schema in the 'parameters.yaml'
+  motion_odometry::Params config_;
 
   void supportCallback(const biped_interfaces::msg::Phase::SharedPtr msg);
   void jointStateCb(const sensor_msgs::msg::JointState::SharedPtr msg);
@@ -50,3 +52,5 @@ class MotionOdometry : public rclcpp::Node {
   std::string current_support_link_;
   rclcpp::Time start_time_;
 };
+
+}

bitbots_odometry/launch/odometry.launch

@@ -6,16 +6,12 @@
     <let unless="$(env IS_ROBOT false)" name="taskset" value=""/>
 
     <node name="motion_odometry" pkg="bitbots_odometry" exec="motion_odometry" launch-prefix="$(var taskset)">
+        <param from="$(find-pkg-share bitbots_odometry)/config/odometry_config_$(env ROBOT_NAME default).yaml"/>
         <param name="base_link_frame" value="$(var tf_prefix)base_link"/>
         <param name="r_sole_frame" value="$(var tf_prefix)r_sole"/>
         <param name="l_sole_frame" value="$(var tf_prefix)l_sole"/>
         <param name="odom_frame" value="$(var tf_prefix)odom"/>
-        <param name="x_forward_scaling" value="1.25"/>
-        <param name="x_backward_scaling" value="0.95"/>
-        <param name="y_scaling" value="1.0"/>
-        <param name="yaw_scaling" value="1.4"/>
         <param name="use_sim_time" value="$(var sim)"/>
-        <param name="publish_walk_odom_tf" value="false"/>
     </node>
 
     <node name="odometry_fuser" pkg="bitbots_odometry" exec="odometry_fuser" launch-prefix="$(var taskset)">

bitbots_odometry/package.xml

@@ -23,6 +23,7 @@
   <depend>rot_conv</depend>
   <depend>bitbots_docs</depend>
   <depend>biped_interfaces</depend>
+  <depend>generate_parameter_library</depend>
 
   <export>
     <bitbots_documentation>