move from Pose to PoseStamped

StoglRobotics-forks · Aug 1, 2024 · 0b25655 · 0b25655
1 parent 5bd5767
commit 0b25655
Show file tree

Hide file tree

Showing 4 changed files with 13 additions and 14 deletions.
diff --git a/admittance_controller/include/admittance_controller/admittance_controller.hpp b/admittance_controller/include/admittance_controller/admittance_controller.hpp
@@ -133,7 +133,7 @@ class AdmittanceController : public controller_interface::ChainableControllerInt
   // ROS subscribers
   rclcpp::Subscription<trajectory_msgs::msg::JointTrajectoryPoint>::SharedPtr
     input_joint_command_subscriber_;
-  rclcpp::Subscription<geometry_msgs::msg::Pose>::SharedPtr
+  rclcpp::Subscription<geometry_msgs::msg::PoseStamped>::SharedPtr
     input_goal_pose_subscriber_;
   rclcpp::Publisher<control_msgs::msg::AdmittanceControllerState>::SharedPtr s_publisher_;
 
@@ -142,12 +142,12 @@ class AdmittanceController : public controller_interface::ChainableControllerInt
 
   // ROS messages
   std::shared_ptr<trajectory_msgs::msg::JointTrajectoryPoint> joint_command_msg_;
-  std::shared_ptr<geometry_msgs::msg::Pose> goal_pose_msg_;
+  std::shared_ptr<geometry_msgs::msg::PoseStamped> goal_pose_msg_;
 
   // real-time buffer
   realtime_tools::RealtimeBuffer<std::shared_ptr<trajectory_msgs::msg::JointTrajectoryPoint>>
     input_joint_command_;
-  realtime_tools::RealtimeBuffer<std::shared_ptr<geometry_msgs::msg::Pose>>
+  realtime_tools::RealtimeBuffer<std::shared_ptr<geometry_msgs::msg::PoseStamped>>
     input_goal_pose_;
   std::unique_ptr<realtime_tools::RealtimePublisher<ControllerStateMsg>> state_publisher_;
 

diff --git a/admittance_controller/include/admittance_controller/admittance_rule.hpp b/admittance_controller/include/admittance_controller/admittance_rule.hpp
@@ -30,6 +30,7 @@
 #include "pluginlib/class_loader.hpp"
 #include "trajectory_msgs/msg/joint_trajectory_point.hpp"
 #include "geometry_msgs/msg/pose.hpp"
+#include "geometry_msgs/msg/pose_stamped.hpp"
 
 namespace admittance_controller
 {
@@ -119,7 +120,7 @@ class AdmittanceRule
    */
   bool get_all_transforms(
     const trajectory_msgs::msg::JointTrajectoryPoint & current_joint_state,
-    const geometry_msgs::msg::Pose & reference_pose);
+    const geometry_msgs::msg::PoseStamped & reference_pose);
 
   /**
    * Updates parameter_ struct if any parameters have changed since last update. Parameter dependent
@@ -142,7 +143,7 @@ class AdmittanceRule
   controller_interface::return_type update(
     const trajectory_msgs::msg::JointTrajectoryPoint & current_joint_state,
     const geometry_msgs::msg::Wrench & measured_wrench,
-    const geometry_msgs::msg::Pose & reference_pose,
+    const geometry_msgs::msg::PoseStamped & reference_pose,
     const trajectory_msgs::msg::JointTrajectoryPoint & reference_joint_state,
     const rclcpp::Duration & period,
     trajectory_msgs::msg::JointTrajectoryPoint & desired_joint_states);

diff --git a/admittance_controller/include/admittance_controller/admittance_rule_impl.hpp b/admittance_controller/include/admittance_controller/admittance_rule_impl.hpp
@@ -148,11 +148,11 @@ void AdmittanceRule::apply_parameters_update()
 
 bool AdmittanceRule::get_all_transforms(
   const trajectory_msgs::msg::JointTrajectoryPoint & current_joint_state,
-  const geometry_msgs::msg::Pose & reference_pose)
+  const geometry_msgs::msg::PoseStamped & reference_pose)
 {
   // get reference transforms 
   bool success=true;  
-  tf2::fromMsg(reference_pose, admittance_transforms_.ref_base_ft_);
+  tf2::fromMsg(reference_pose.pose, admittance_transforms_.ref_base_ft_);
 
   // get transforms at current configuration
   success &= kinematics_->calculate_link_transform(
@@ -176,7 +176,7 @@ bool AdmittanceRule::get_all_transforms(
 controller_interface::return_type AdmittanceRule::update(
   const trajectory_msgs::msg::JointTrajectoryPoint & current_joint_state,
   const geometry_msgs::msg::Wrench & measured_wrench,
-  const geometry_msgs::msg::Pose & reference_pose,
+  const geometry_msgs::msg::PoseStamped & reference_pose,
   const trajectory_msgs::msg::JointTrajectoryPoint & reference_joint_state, 
   const rclcpp::Duration & period, trajectory_msgs::msg::JointTrajectoryPoint & desired_joint_state)
 {

diff --git a/admittance_controller/src/admittance_controller.cpp b/admittance_controller/src/admittance_controller.cpp
@@ -267,10 +267,10 @@ controller_interface::CallbackReturn AdmittanceController::on_configure(
       "~/joint_references", rclcpp::SystemDefaultsQoS(), joint_command_callback);
 
   auto goal_pose_callback =
-    [this](const std::shared_ptr<geometry_msgs::msg::Pose> msg)
+    [this](const std::shared_ptr<geometry_msgs::msg::PoseStamped> msg)
   { input_goal_pose_.writeFromNonRT(msg); };
   input_goal_pose_subscriber_ =
-    get_node()->create_subscription<geometry_msgs::msg::Pose>(
+    get_node()->create_subscription<geometry_msgs::msg::PoseStamped>(
       "~/goal_pose", rclcpp::SystemDefaultsQoS(), goal_pose_callback);
 
   s_publisher_ = get_node()->create_publisher<control_msgs::msg::AdmittanceControllerState>(
@@ -354,10 +354,8 @@ controller_interface::CallbackReturn AdmittanceController::on_activate(
       return controller_interface::CallbackReturn::ERROR;
     }
   }
-
-  goal_pose_msg_ = std::make_shared<geometry_msgs::msg::Pose>(
-    admittance_->initialize_goal_pose(joint_state_)
-    );
+  goal_pose_msg_ = std::make_shared<geometry_msgs::msg::PoseStamped>();
+  goal_pose_msg_->pose = admittance_->initialize_goal_pose(joint_state_);
   if(!goal_pose_msg_){
     RCLCPP_ERROR(get_node()->get_logger(), "Failed to initialize goal_pose from current joint positions.\n");
     return controller_interface::CallbackReturn::ERROR;