From 65ac77c13ea9e73421d56b0c9a4d5c68116c4b2a Mon Sep 17 00:00:00 2001 From: Kenji Brameld Date: Fri, 8 Sep 2023 03:42:40 +0000 Subject: [PATCH 1/5] try using a Walk action, rather than a topic Signed-off-by: Kenji Brameld --- walk/include/walk/walk.hpp | 14 +++++++++++ walk/src/walk.cpp | 38 ++++++++++++++++++++++++++++++ walk_interfaces/CMakeLists.txt | 3 +++ walk_interfaces/action/Walk.action | 3 +++ 4 files changed, 58 insertions(+) create mode 100644 walk_interfaces/action/Walk.action diff --git a/walk/include/walk/walk.hpp b/walk/include/walk/walk.hpp index 60bff0b..289d012 100644 --- a/walk/include/walk/walk.hpp +++ b/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::SharedPtr pub_current_twist_; rclcpp::Publisher::SharedPtr pub_ready_to_step_; + // Action Servers + rclcpp_action::Server::SharedPtr action_server_walk_; + // Debug publishers rclcpp::Publisher::SharedPtr pub_gait_; rclcpp::Publisher::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 goal); + rclcpp_action::CancelResponse handleCancelWalk( + const std::shared_ptr> goal_handle); + void handleAcceptedWalk( + const std::shared_ptr> goal_handle); // Parameters std::unique_ptr params_; @@ -84,6 +95,9 @@ class Walk : public rclcpp::Node geometry_msgs::msg::Twist target_twist_; std::unique_ptr step_; std::unique_ptr step_state_; + + bool active_ = false; + std::shared_ptr> goal_handle_; }; } // namespace walk diff --git a/walk/src/walk.cpp b/walk/src/walk.cpp index 2916af8..08d08bd 100644 --- a/walk/src/walk.cpp +++ b/walk/src/walk.cpp @@ -25,6 +25,7 @@ #include #include +#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("walk/current_twist", 1); pub_ready_to_step_ = create_publisher("walk/ready_to_step", 1); + action_server_walk_ = rclcpp_action::create_server( + 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/gait", 1); pub_step_ = create_publisher("walk/step", 1); } @@ -72,6 +80,9 @@ void Walk::generateCommand() { RCLCPP_DEBUG(get_logger(), "generateCommand()"); + if (!active_) + return; + if (!step_) { RCLCPP_INFO_THROTTLE( get_logger(), *get_clock(), 1000, // ms @@ -142,6 +153,33 @@ void Walk::imuCallback(const sensor_msgs::msg::Imu & imu) filtered_gyro_y_ = 0.8 * filtered_gyro_y_ + 0.2 * imu.angular_velocity.y; } +rclcpp_action::GoalResponse Walk::handleGoalWalk( + const rclcpp_action::GoalUUID & uuid, + std::shared_ptr 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> 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> goal_handle) +{ + target_twist_ = twist_limiter::limit(params_->twist_limiter_, goal_handle->get_goal()->twist); + goal_handle_ = goal_handle; +} + + } // namespace walk #include "rclcpp_components/register_node_macro.hpp" diff --git a/walk_interfaces/CMakeLists.txt b/walk_interfaces/CMakeLists.txt index f0cdf93..1ea7b6d 100644 --- a/walk_interfaces/CMakeLists.txt +++ b/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) diff --git a/walk_interfaces/action/Walk.action b/walk_interfaces/action/Walk.action new file mode 100644 index 0000000..48e842e --- /dev/null +++ b/walk_interfaces/action/Walk.action @@ -0,0 +1,3 @@ +geometry_msgs/Twist twist +--- +--- From 838fe3533cc33fc1768605eb1164c075a6cf8dfd Mon Sep 17 00:00:00 2001 From: Kenji Brameld Date: Wed, 13 Sep 2023 04:10:29 +0000 Subject: [PATCH 2/5] set active_ to true if received message on walk topic Signed-off-by: Kenji Brameld --- walk/src/walk.cpp | 1 + 1 file changed, 1 insertion(+) diff --git a/walk/src/walk.cpp b/walk/src/walk.cpp index 08d08bd..b1f9143 100644 --- a/walk/src/walk.cpp +++ b/walk/src/walk.cpp @@ -105,6 +105,7 @@ void Walk::generateCommand() void Walk::walk(const geometry_msgs::msg::Twist & commanded_twist) { + active_ = true; RCLCPP_DEBUG( get_logger(), "walk() called with commanded_twist: %.3f, %.3f, %.3f, %.3f, %.3f, %.3f", commanded_twist.linear.x, commanded_twist.linear.y, commanded_twist.linear.z, From bb80d764be33323102f39ee0106bf31e1de2b9d2 Mon Sep 17 00:00:00 2001 From: Kenji Brameld Date: Fri, 15 Sep 2023 04:17:14 +0000 Subject: [PATCH 3/5] don't filter gyro, because we can be more aggressive in simulator Signed-off-by: Kenji Brameld --- walk/src/walk.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/walk/src/walk.cpp b/walk/src/walk.cpp index b1f9143..8a40062 100644 --- a/walk/src/walk.cpp +++ b/walk/src/walk.cpp @@ -151,7 +151,7 @@ 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( From 98abd1dc69fa7529652a1623204b47b260c8e4c6 Mon Sep 17 00:00:00 2001 From: Kenji Brameld Date: Sat, 16 Sep 2023 17:09:45 +0000 Subject: [PATCH 4/5] disable info log Signed-off-by: Kenji Brameld --- walk/src/walk.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/walk/src/walk.cpp b/walk/src/walk.cpp index 8a40062..16fa350 100644 --- a/walk/src/walk.cpp +++ b/walk/src/walk.cpp @@ -158,7 +158,7 @@ rclcpp_action::GoalResponse Walk::handleGoalWalk( const rclcpp_action::GoalUUID & uuid, std::shared_ptr goal) { - RCLCPP_INFO(get_logger(), "Received goal request"); + // RCLCPP_INFO(get_logger(), "Received goal request"); (void)uuid; (void)goal; active_ = true; @@ -167,7 +167,7 @@ rclcpp_action::GoalResponse Walk::handleGoalWalk( rclcpp_action::CancelResponse Walk::handleCancelWalk( const std::shared_ptr> goal_handle) { - RCLCPP_INFO(get_logger(), "Received request to cancel goal"); + // RCLCPP_INFO(get_logger(), "Received request to cancel goal"); (void)goal_handle; active_ = false; goal_handle_.reset(); From d0597bac73cfcfcf437236f42e3d6ea6cc0b3870 Mon Sep 17 00:00:00 2001 From: Kenji Brameld Date: Thu, 21 Sep 2023 06:24:51 +0000 Subject: [PATCH 5/5] don't set walk to active on subscription Signed-off-by: Kenji Brameld --- walk/src/walk.cpp | 3 +-- 1 file changed, 1 insertion(+), 2 deletions(-) diff --git a/walk/src/walk.cpp b/walk/src/walk.cpp index 16fa350..0991342 100644 --- a/walk/src/walk.cpp +++ b/walk/src/walk.cpp @@ -78,7 +78,7 @@ Walk::~Walk() {} void Walk::generateCommand() { - RCLCPP_DEBUG(get_logger(), "generateCommand()"); + // RCLCPP_DEBUG(get_logger(), "generateCommand()"); if (!active_) return; @@ -105,7 +105,6 @@ void Walk::generateCommand() void Walk::walk(const geometry_msgs::msg::Twist & commanded_twist) { - active_ = true; RCLCPP_DEBUG( get_logger(), "walk() called with commanded_twist: %.3f, %.3f, %.3f, %.3f, %.3f, %.3f", commanded_twist.linear.x, commanded_twist.linear.y, commanded_twist.linear.z,