Skip to content

Commit

Permalink
move from Pose to PoseStamped
Browse files Browse the repository at this point in the history
  • Loading branch information
Nibanovic committed Aug 1, 2024
1 parent 5bd5767 commit 0b25655
Show file tree
Hide file tree
Showing 4 changed files with 13 additions and 14 deletions.
Original file line number Diff line number Diff line change
Expand Up @@ -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_;

Expand All @@ -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_;

Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -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
{
Expand Down Expand Up @@ -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
Expand All @@ -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);
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -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(
Expand All @@ -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)
{
Expand Down
10 changes: 4 additions & 6 deletions admittance_controller/src/admittance_controller.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -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>(
Expand Down Expand Up @@ -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;
Expand Down

0 comments on commit 0b25655

Please sign in to comment.