try using a Walk action, rather than a topic

walk/include/walk/walk.hpp

@@ -27,6 +27,7 @@
 #include "std_msgs/msg/bool.hpp"
 #include "walk_interfaces/action/crouch.hpp"
 #include "walk_interfaces/action/stand.hpp"
+#include "walk_interfaces/action/walk.hpp"
 #include "walk_interfaces/msg/feet_trajectory_point.hpp"
 #include "walk_interfaces/msg/gait.hpp"
 #include "walk_interfaces/msg/step.hpp"
@@ -61,6 +62,9 @@ class Walk : public rclcpp::Node
   rclcpp::Publisher<geometry_msgs::msg::Twist>::SharedPtr pub_current_twist_;
   rclcpp::Publisher<std_msgs::msg::Bool>::SharedPtr pub_ready_to_step_;
 
+  // Action Servers
+  rclcpp_action::Server<walk_interfaces::action::Walk>::SharedPtr action_server_walk_;
+
   // Debug publishers
   rclcpp::Publisher<walk_interfaces::msg::Gait>::SharedPtr pub_gait_;
   rclcpp::Publisher<walk_interfaces::msg::Step>::SharedPtr pub_step_;
@@ -71,6 +75,13 @@ class Walk : public rclcpp::Node
   void generateCommand();
   void phaseCallback(const biped_interfaces::msg::Phase::SharedPtr msg);
   void targetCallback(const geometry_msgs::msg::Twist::SharedPtr msg);
+  rclcpp_action::GoalResponse handleGoalWalk(
+    const rclcpp_action::GoalUUID & uuid,
+    std::shared_ptr<const walk_interfaces::action::Walk::Goal> goal);
+  rclcpp_action::CancelResponse handleCancelWalk(
+    const std::shared_ptr<rclcpp_action::ServerGoalHandle<walk_interfaces::action::Walk>> goal_handle);
+  void handleAcceptedWalk(
+    const std::shared_ptr<rclcpp_action::ServerGoalHandle<walk_interfaces::action::Walk>> goal_handle);
 
   // Parameters
   std::unique_ptr<Params> params_;
@@ -84,6 +95,9 @@ class Walk : public rclcpp::Node
   geometry_msgs::msg::Twist target_twist_;
   std::unique_ptr<walk_interfaces::msg::Step> step_;
   std::unique_ptr<StepState> step_state_;
+
+  bool active_ = false;
+  std::shared_ptr<rclcpp_action::ServerGoalHandle<walk_interfaces::action::Walk>> goal_handle_;
 };
 
 }  // namespace walk

walk/src/walk.cpp

@@ -25,6 +25,7 @@
 #include <memory>
 #include <utility>
 
+#include "rclcpp_action/rclcpp_action.hpp"
 #include "walk/walk.hpp"
 #include "twist_limiter.hpp"
 #include "twist_change_limiter.hpp"
@@ -62,6 +63,13 @@ Walk::Walk(const rclcpp::NodeOptions & options)
   pub_current_twist_ = create_publisher<geometry_msgs::msg::Twist>("walk/current_twist", 1);
   pub_ready_to_step_ = create_publisher<std_msgs::msg::Bool>("walk/ready_to_step", 1);
 
+  action_server_walk_ = rclcpp_action::create_server<walk_interfaces::action::Walk>(
+    this,
+    "walk",
+    std::bind(&Walk::handleGoalWalk, this, std::placeholders::_1, std::placeholders::_2),
+    std::bind(&Walk::handleCancelWalk, this, std::placeholders::_1),
+    std::bind(&Walk::handleAcceptedWalk, this, std::placeholders::_1));
+
   pub_gait_ = create_publisher<walk_interfaces::msg::Gait>("walk/gait", 1);
   pub_step_ = create_publisher<walk_interfaces::msg::Step>("walk/step", 1);
 }
@@ -70,7 +78,10 @@ Walk::~Walk() {}
 
 void Walk::generateCommand()
 {
-  RCLCPP_DEBUG(get_logger(), "generateCommand()");
+  // RCLCPP_DEBUG(get_logger(), "generateCommand()");
+
+  if (!active_)
+    return;
 
   if (!step_) {
     RCLCPP_INFO_THROTTLE(
@@ -139,8 +150,35 @@ void Walk::notifyPhase(const biped_interfaces::msg::Phase & phase)
 
 void Walk::imuCallback(const sensor_msgs::msg::Imu & imu)
 {
-  filtered_gyro_y_ = 0.8 * filtered_gyro_y_ + 0.2 * imu.angular_velocity.y;
+  filtered_gyro_y_ = imu.angular_velocity.y;
+}
+
+rclcpp_action::GoalResponse Walk::handleGoalWalk(
+  const rclcpp_action::GoalUUID & uuid,
+  std::shared_ptr<const walk_interfaces::action::Walk::Goal> goal)
+{
+  // RCLCPP_INFO(get_logger(), "Received goal request");
+  (void)uuid;
+  (void)goal;
+  active_ = true;
+  return rclcpp_action::GoalResponse::ACCEPT_AND_EXECUTE;
 }
+rclcpp_action::CancelResponse Walk::handleCancelWalk(
+  const std::shared_ptr<rclcpp_action::ServerGoalHandle<walk_interfaces::action::Walk>> goal_handle)
+{
+  // RCLCPP_INFO(get_logger(), "Received request to cancel goal");
+  (void)goal_handle;
+  active_ = false;
+  goal_handle_.reset();
+  return rclcpp_action::CancelResponse::ACCEPT;
+}
+void Walk::handleAcceptedWalk(
+  const std::shared_ptr<rclcpp_action::ServerGoalHandle<walk_interfaces::action::Walk>> goal_handle)
+{
+  target_twist_ = twist_limiter::limit(params_->twist_limiter_, goal_handle->get_goal()->twist);
+  goal_handle_ = goal_handle;
+}
+
 
 }  // namespace walk
 

walk_interfaces/CMakeLists.txt

@@ -7,14 +7,17 @@ endif()
 
 # find dependencies
 find_package(ament_cmake REQUIRED)
+find_package(geometry_msgs REQUIRED)
 find_package(rosidl_default_generators REQUIRED)
 
 rosidl_generate_interfaces(${PROJECT_NAME}
   "action/Stand.action"
   "action/Crouch.action"
+  "action/Walk.action"
   "msg/FeetTrajectoryPoint.msg"
   "msg/Gait.msg"
   "msg/Step.msg"
+  DEPENDENCIES geometry_msgs
 )
 
 ament_export_dependencies(rosidl_default_runtime)

walk_interfaces/action/Walk.action

@@ -0,0 +1,3 @@
+geometry_msgs/Twist twist
+---
+---