Walking patterns, Xbox controller, and mrore

base_twerk/include/base_twerk/base_twerk_action_server.hpp

@@ -9,13 +9,15 @@
 
 #include "base_twerk_msgs/action/base_twerk_action.hpp"
 #include "base_twerk_msgs/srv/base_pose_setpoint.hpp"
+#include "base_twerk_msgs/srv/get_current_base_pose.hpp"
 #include "geometry_msgs/msg/pose_stamped.hpp"
 
 namespace penta_pod::kin::base_twerk {
 
 using BaseTwerkAction = base_twerk_msgs::action::BaseTwerkAction;
 using GoalHandle = rclcpp_action::ServerGoalHandle<BaseTwerkAction>;
 using BasePoseSetpointSrv = base_twerk_msgs::srv::BasePoseSetpoint;
+using GetCurrentBasePose = base_twerk_msgs::srv::GetCurrentBasePose;
 using geometry_msgs::msg::PoseStamped;
 using penta_pod::kin::commons::service_call_template;
 
@@ -27,7 +29,7 @@ class BaseTwerkActionServer {
 
   rclcpp::Client<BasePoseSetpointSrv>::SharedPtr setpoint_client_;
 
-  rclcpp::Subscription<PoseStamped>::SharedPtr base_pose_subscriber_;
+  rclcpp::Client<GetCurrentBasePose>::SharedPtr get_currnet_pose_client_;
 
   rclcpp::CallbackGroup::SharedPtr callback_group_;
 
@@ -46,9 +48,12 @@ class BaseTwerkActionServer {
   auto get_parameters() -> void;
 
   auto call_setpoint_client(const PoseStamped &base_to_basefootprint) -> bool;
+
   auto calculate_twerk_pose_from_goal(
       rclcpp::Time start_time, PoseStamped start_pose,
       const std::shared_ptr<GoalHandle> goal_handle) -> PoseStamped;
+
+  auto quiry_current_base_pose() -> std::optional<GetCurrentBasePose::Response>;
 
   double update_interval_millis_double_;
   double max_permissible_displacement_meter_;

base_twerk/include/base_twerk/base_twerk_cmd_publisher.hpp

@@ -6,18 +6,23 @@
 
 // include messages
 #include "base_twerk_msgs/srv/base_pose_setpoint.hpp"
+#include "base_twerk_msgs/srv/get_current_base_pose.hpp"
 #include "geometry_msgs/msg/pose_stamped.hpp"
 
 namespace penta_pod::kin::base_twerk {
 
 using PoseStamped = geometry_msgs::msg::PoseStamped;
 using BasePoseSetpoint = base_twerk_msgs::srv::BasePoseSetpoint;
+using GetCurrentBasePose = base_twerk_msgs::srv::GetCurrentBasePose;
+
+const auto base_footprint = "base_footprint";
 
 class BaseTwerkCmdPuplisher {
 private:
   rclcpp::Node::SharedPtr node_;
   rclcpp::Publisher<PoseStamped>::SharedPtr base_pose_publisher_;
   rclcpp::Service<BasePoseSetpoint>::SharedPtr setpoint_service_;
+  rclcpp::Service<GetCurrentBasePose>::SharedPtr get_current_base_pose_;
   rclcpp::CallbackGroup::SharedPtr callback_group_;
   rclcpp::TimerBase::SharedPtr timer_;
 
@@ -31,6 +36,7 @@ class BaseTwerkCmdPuplisher {
   void load_parameters();
   void timer_callback();
   void create_setpoint_service();
+  void create_get_current_base_pose_service();
 
 public:
   explicit BaseTwerkCmdPuplisher();

base_twerk/launch/base_twerk_action_server.launch.py

@@ -3,17 +3,20 @@
 from launch import LaunchDescription
 from launch_ros.actions import Node
 
+
 def generate_launch_description():
     config = os.path.join(
         get_package_share_directory("penta_description"),
         "config",
         "general_config.yaml",
     )
     ld = LaunchDescription()
-    ld.add_action(Node(
-        package="base_twerk",
-        executable="base_twerk_action_server_node",
-        output="screen",
-        parameters=[config],
-    ))
+    ld.add_action(
+        Node(
+            package="base_twerk",
+            executable="base_twerk_action_server_node",
+            output="screen",
+            parameters=[config],
+        )
+    )
     return ld

base_twerk/launch/null_pose_publisher.launch.py

@@ -3,17 +3,20 @@
 from launch import LaunchDescription
 from launch_ros.actions import Node
 
+
 def generate_launch_description():
     config = os.path.join(
         get_package_share_directory("penta_description"),
         "config",
         "general_config.yaml",
     )
     ld = LaunchDescription()
-    ld.add_action(Node(
-        package="base_twerk",
-        executable="base_twerk_publisher_node",
-        output="screen",
-        parameters=[config],
-    ))
+    ld.add_action(
+        Node(
+            package="base_twerk",
+            executable="base_twerk_publisher_node",
+            output="screen",
+            parameters=[config],
+        )
+    )
     return ld

base_twerk/src/base_twerk/base_twerk_action_server.cpp

@@ -1,4 +1,5 @@
 #include "base_twerk/base_twerk_action_server.hpp"
+#include "commons/quaternion_utils.hpp"
 
 namespace penta_pod::kin::base_twerk {
 
@@ -16,14 +17,9 @@ BaseTwerkActionServer::BaseTwerkActionServer()
   this->setpoint_client_ = node_->create_client<BasePoseSetpointSrv>(
       "cmd_null_setpoint", rmw_qos_profile_services_default, callback_group_);
 
-  this->base_pose_subscriber_ = node_->create_subscription<PoseStamped>(
-      "null_space_pose", 10, [this](const PoseStamped &msg) -> void {
-        {
-          std::lock_guard<std::mutex> lock(received_base_pose_mutex_);
-          received_base_pose_.timestamp = node_->now();
-          received_base_pose_.value = msg;
-        }
-      });
+  this->get_currnet_pose_client_ = node_->create_client<GetCurrentBasePose>(
+      "get_current_null_pose", rmw_qos_profile_services_default,
+      callback_group_);
 
   this->base_twerk_action_server_ =
       rclcpp_action::create_server<BaseTwerkAction>(
@@ -65,8 +61,9 @@ auto BaseTwerkActionServer::execute(
   const auto goal = goal_handle->get_goal();
   auto result = std::make_shared<BaseTwerkAction::Result>();
 
-  double mag_displacement = std::sqrt(
-      goal->rx * goal->rx + goal->ry * goal->ry + goal->rz * goal->rz);
+  double mag_displacement =
+      std::sqrt(goal->r[0] * goal->r[0] + goal->r[1] * goal->r[1] +
+                goal->r[2] * goal->r[2]);
   if (mag_displacement > max_permissible_displacement_meter_) {
     auto message = "Action aborted for the specified displacement is: " +
                    std::to_string(mag_displacement) +
@@ -96,26 +93,19 @@ auto BaseTwerkActionServer::execute(
     return false;
   };
 
-  // get current null pose
-  auto start_time = node_->now();
-  while (rclcpp::ok()) {
-    rate.sleep();
-
-    if (received_base_pose_.timestamp > start_time) {
-      break;
-    }
-
-    if (is_timed_out(start_time, 1s)) {
-      result->result_message = "Timed out, did not receive transform message";
-      goal_handle->abort(result);
-      RCLCPP_WARN(node_->get_logger(), "Time out, breaking out from action");
-      return;
-    }
+  // get current null space pose
+  auto get_currnet_pose_response = quiry_current_base_pose();
+  if (!get_currnet_pose_response.has_value()) {
+    result->result_message = "Failed to get current base pose";
+    goal_handle->abort(result);
+    return;
   }
 
   PoseStamped start_pose{};
   {
     std::lock_guard<std::mutex> lock(received_base_pose_mutex_);
+    received_base_pose_ = {node_->now(),
+                           get_currnet_pose_response.value().pose};
     start_pose = received_base_pose_.value;
     RCLCPP_INFO(node_->get_logger(), "Current base pose is: x:%f y:%f z:%f",
                 start_pose.pose.position.x, start_pose.pose.position.y,
@@ -126,13 +116,13 @@ auto BaseTwerkActionServer::execute(
       std::chrono::milliseconds(static_cast<int>(goal->dance_time_millis));
 
   // start control loop for base pose motion
-  start_time = node_->now();
+  auto start_time = node_->now();
   while (rclcpp::ok()) {
     rate.sleep();
 
-    auto current_pose =
+    auto updated_pose =
         calculate_twerk_pose_from_goal(start_time, start_pose, goal_handle);
-    if (!call_setpoint_client(current_pose)) {
+    if (!call_setpoint_client(updated_pose)) {
       return;
     }
 
@@ -148,6 +138,9 @@ auto BaseTwerkActionServer::execute(
     }
 
     if (goal_handle->is_canceling()) {
+      if (!call_setpoint_client(start_pose)) {
+        return;
+      }
       result->result_message = "action canceled";
       goal_handle->canceled(result);
       RCLCPP_INFO(node_->get_logger(), "Goal canceled");
@@ -156,6 +149,54 @@ auto BaseTwerkActionServer::execute(
   }
 }
 
+auto BaseTwerkActionServer::quiry_current_base_pose()
+    -> std::optional<GetCurrentBasePose::Response> {
+
+  // check if service is available
+  auto service_name = get_currnet_pose_client_->get_service_name();
+  if (!get_currnet_pose_client_->wait_for_service(1s)) {
+    RCLCPP_ERROR(node_->get_logger(), "Service %s not online!", service_name);
+    return std::nullopt;
+  }
+
+  // send request
+  auto get_current_pose_request =
+      std::make_shared<GetCurrentBasePose::Request>();
+
+  auto start_time = node_->now();
+
+  auto future =
+      get_currnet_pose_client_->async_send_request(get_current_pose_request);
+
+  using namespace std::chrono_literals;
+  const auto timeout_millis = std::chrono::milliseconds(500);
+
+  auto status = future.wait_for(timeout_millis);
+
+  if (status != std::future_status::ready) {
+    RCLCPP_ERROR(node_->get_logger(), "Service %s response timed out!",
+                 service_name);
+    return std::nullopt;
+  }
+
+  if (!future.valid()) {
+    RCLCPP_ERROR(node_->get_logger(), "Future from service %s is not valid!",
+                 service_name);
+    return std::nullopt;
+  }
+
+  auto result = future.get();
+  if (!result) {
+    RCLCPP_ERROR(node_->get_logger(), "Returned null Service %s response!",
+                 service_name);
+    return std::nullopt;
+  }
+
+  RCLCPP_INFO(node_->get_logger(), "Service %s response is ready!",
+              service_name);
+  return *result;
+}
+
 auto BaseTwerkActionServer::calculate_twerk_pose_from_goal(
     rclcpp::Time start_time, PoseStamped start_pose,
     const std::shared_ptr<GoalHandle> goal_handle) -> PoseStamped {
@@ -166,14 +207,23 @@ auto BaseTwerkActionServer::calculate_twerk_pose_from_goal(
 
   pose.pose.position.x =
       start_pose.pose.position.x +
-      goal->rx * sin(goal->w * delta_t_seconds + goal->phi_x);
+      goal->r[0] * sin(goal->w * delta_t_seconds + goal->phi[0]);
   pose.pose.position.y =
       start_pose.pose.position.y +
-      goal->ry * sin(goal->w * delta_t_seconds + goal->phi_y);
+      goal->r[1] * sin(goal->w * delta_t_seconds + goal->phi[1]);
   pose.pose.position.z =
       start_pose.pose.position.z +
-      goal->rz * sin(goal->w * delta_t_seconds + goal->phi_z);
+      goal->r[2] * sin(goal->w * delta_t_seconds + goal->phi[2]);
+
+  std::vector<double> rpy = {0., 0., 0.};
+  for (int i = 3; i < 6; i++) {
+    rpy[i - 3] = goal->r[i] * sin(goal->w * delta_t_seconds + goal->phi[i]);
+  }
 
+  auto roll = rpy[0];
+  auto pitch = rpy[1];
+  auto yaw = rpy[2];
+  pose.pose.orientation = rpy_to_quaternion(yaw, pitch, roll);
   return pose;
 }
 

base_twerk/src/base_twerk/base_twerk_cmd_publisher.cpp

@@ -20,17 +20,18 @@ BaseTwerkCmdPuplisher::BaseTwerkCmdPuplisher()
       std::chrono::milliseconds(static_cast<int>(update_interval_millis_)),
       [this]() { timer_callback(); });
 
+  callback_group_ = node_->create_callback_group(
+      rclcpp::CallbackGroupType::MutuallyExclusive);
+
   create_setpoint_service();
+  create_get_current_base_pose_service();
 }
 
 void BaseTwerkCmdPuplisher::create_setpoint_service() {
-  callback_group_ = node_->create_callback_group(
-      rclcpp::CallbackGroupType::MutuallyExclusive);
   auto lambda =
       [this](const BasePoseSetpoint::Request::SharedPtr &request,
              const BasePoseSetpoint::Response::SharedPtr &response) -> bool {
     auto frame_id = request->pose.header.frame_id;
-    const auto base_footprint = "base_footprint";
     if (frame_id == "") {
       frame_id = base_footprint;
     }
@@ -54,6 +55,22 @@ void BaseTwerkCmdPuplisher::create_setpoint_service() {
       callback_group_);
 }
 
+void BaseTwerkCmdPuplisher::create_get_current_base_pose_service() {
+  auto lambda =
+      [this](const GetCurrentBasePose::Request::SharedPtr & /*request*/,
+             const GetCurrentBasePose::Response::SharedPtr &response) -> bool {
+    auto pose = base_pose_;
+    pose.header.stamp = node_->now();
+    pose.header.frame_id = base_footprint;
+    response->pose = pose;
+    return true;
+  };
+
+  get_current_base_pose_ = node_->create_service<GetCurrentBasePose>(
+      "get_current_null_pose", lambda, rmw_qos_profile_services_default,
+      callback_group_);
+}
+
 void BaseTwerkCmdPuplisher::timer_callback() {
   double deta_t_sec = update_interval_millis_ / 1000.0;
   using namespace penta_pod::kin::commons;

commons/include/commons/quaternion_utils.hpp

@@ -0,0 +1,38 @@
+#ifndef COMMONS_QUAT_UTILS_HPP_
+#define COMMONS_QUAT_UTILS_HPP_
+
+#include "geometry_msgs/msg/quaternion.hpp"
+
+using geometry_msgs::msg::Quaternion;
+
+Quaternion rpy_to_quaternion(double yaw, double pitch, double roll) // yaw (Z), pitch (Y), roll (X)
+{
+  double cy = cos(yaw * 0.5);
+  double sy = sin(yaw * 0.5);
+  double cp = cos(pitch * 0.5);
+  double sp = sin(pitch * 0.5);
+  double cr = cos(roll * 0.5);
+  double sr = sin(roll * 0.5);
+
+  Quaternion q;
+  q.w = cy * cp * cr + sy * sp * sr;
+  q.x = cy * cp * sr - sy * sp * cr;
+  q.y = sy * cp * sr + cy * sp * cr;
+  q.z = sy * cp * cr - cy * sp * sr;
+  return q;
+}
+
+Quaternion hamilton_product(Quaternion u, Quaternion v)
+{
+    Quaternion result;
+
+    result.w = u.w*v.w - u.x*v.x - u.y*v.y - u.z*v.z;
+    result.x = u.w*v.x + u.x*v.w + u.y*v.z - u.z*v.y;
+    result.y = u.w*v.y - u.x*v.z + u.y*v.w + u.z*v.x;
+    result.z = u.w*v.z + u.x*v.y - u.y*v.x + u.z*v.w;
+
+    return result;
+
+}
+
+#endif // COMMONS_QUAT_UTILS_HPP_