Skip to content

Commit

Permalink
Compiles again
Browse files Browse the repository at this point in the history
  • Loading branch information
Lennart Nachtigall committed Nov 21, 2024
1 parent f9a3087 commit 06ff415
Show file tree
Hide file tree
Showing 3 changed files with 10 additions and 11 deletions.
Original file line number Diff line number Diff line change
Expand Up @@ -102,8 +102,7 @@ class AdmittanceRule

/// Configure admittance rule memory using number of joints.
controller_interface::return_type configure(
const std::shared_ptr<rclcpp_lifecycle::LifecycleNode> & node, const size_t num_joint,
const std::string & robot_description);
const std::shared_ptr<rclcpp_lifecycle::LifecycleNode> & node, const size_t num_joint);

/// Reset all values back to default
controller_interface::return_type reset(const size_t num_joints);
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -35,8 +35,7 @@ constexpr auto NUM_CARTESIAN_DOF = 6; // (3 translation + 3 rotation)

/// Configure admittance rule memory for num joints and load kinematics interface
controller_interface::return_type AdmittanceRule::configure(
const std::shared_ptr<rclcpp_lifecycle::LifecycleNode> & node, const size_t num_joints,
const std::string & robot_description)
const std::shared_ptr<rclcpp_lifecycle::LifecycleNode> & node, const size_t num_joints)
{
num_joints_ = num_joints;

Expand All @@ -59,8 +58,7 @@ controller_interface::return_type AdmittanceRule::configure(
kinematics_ = std::unique_ptr<kinematics_interface::KinematicsInterface>(
kinematics_loader_->createUnmanagedInstance(parameters_.kinematics.plugin_name));

if (!kinematics_->initialize(
robot_description, node->get_node_parameters_interface(), "kinematics"))
if (!kinematics_->initialize(node->get_node_parameters_interface(), "kinematics"))
{
return controller_interface::return_type::ERROR;
}
Expand Down
12 changes: 7 additions & 5 deletions admittance_controller/src/admittance_controller.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -289,10 +289,14 @@ controller_interface::CallbackReturn AdmittanceController::on_configure(
"~/wrench_reference", rclcpp::SystemDefaultsQoS(),
[&](const geometry_msgs::msg::WrenchStamped & msg)
{
if (msg.header.frame_id != admittance_->parameters_.ft_sensor.frame.id && !msg.header.frame_id.empty())
if (
msg.header.frame_id != admittance_->parameters_.ft_sensor.frame.id &&
!msg.header.frame_id.empty())
{
RCLCPP_ERROR_STREAM(
get_node()->get_logger(), "Ignoring wrench reference as it is on the wrong frame: " << msg.header.frame_id << ". Expected reference frame: " << admittance_->parameters_.ft_sensor.frame_id);
get_node()->get_logger(), "Ignoring wrench reference as it is on the wrong frame: "
<< msg.header.frame_id << ". Expected reference frame: "
<< admittance_->parameters_.ft_sensor.frame.id);
return;
}
input_wrench_command_.writeFromNonRT(msg);
Expand All @@ -312,9 +316,7 @@ controller_interface::CallbackReturn AdmittanceController::on_configure(
semantic_components::ForceTorqueSensor(admittance_->parameters_.ft_sensor.name));

// configure admittance rule
if (
admittance_->configure(get_node(), num_joints_, this->get_robot_description()) ==
controller_interface::return_type::ERROR)
if (admittance_->configure(get_node(), num_joints_) == controller_interface::return_type::ERROR)
{
return controller_interface::CallbackReturn::ERROR;
}
Expand Down

0 comments on commit 06ff415

Please sign in to comment.