From fe22d3b08d7edf4afab342c8954d868820d8925d Mon Sep 17 00:00:00 2001 From: Luca Beber Date: Thu, 22 Feb 2024 09:49:02 +0100 Subject: [PATCH] small fix --- cartesian_controller_base/src/cartesian_controller_base.cpp | 1 + end_effector_controller/src/end_effector_control.cpp | 5 ++++- 2 files changed, 5 insertions(+), 1 deletion(-) diff --git a/cartesian_controller_base/src/cartesian_controller_base.cpp b/cartesian_controller_base/src/cartesian_controller_base.cpp index cb488d9c..b0918090 100644 --- a/cartesian_controller_base/src/cartesian_controller_base.cpp +++ b/cartesian_controller_base/src/cartesian_controller_base.cpp @@ -293,6 +293,7 @@ CartesianControllerBase::on_deactivate(const rclcpp_lifecycle::State & previous_ m_joint_cmd_pos_handles.clear(); m_joint_cmd_vel_handles.clear(); m_joint_state_pos_handles.clear(); + m_joint_state_vel_handles.clear(); this->release_interfaces(); m_active = false; } diff --git a/end_effector_controller/src/end_effector_control.cpp b/end_effector_controller/src/end_effector_control.cpp index cf6bfb24..077b279a 100644 --- a/end_effector_controller/src/end_effector_control.cpp +++ b/end_effector_controller/src/end_effector_control.cpp @@ -515,6 +515,8 @@ geometry_msgs::msg::PoseStamped EndEffectorControl::getEndEffectorPose() velocities(i) = m_joint_state_vel_handles[i].get().get_value(); } + // Print velocities + RCLCPP_INFO_STREAM(get_node()->get_logger(), "vel arrr: " << velocities.data); KDL::JntArrayVel joint_data(positions, velocities); KDL::FrameVel tmp; m_fk_solver->JntToCart(joint_data, tmp); @@ -525,10 +527,11 @@ geometry_msgs::msg::PoseStamped EndEffectorControl::getEndEffectorPose() current.pose.position.z = tmp.p.p.z(); tmp.M.R.GetQuaternion(current.pose.orientation.x, current.pose.orientation.y, current.pose.orientation.z, current.pose.orientation.w); - + cartVel(0) = tmp.p.v.x(); cartVel(1) = tmp.p.v.y(); cartVel(2) = tmp.p.v.z(); + return current; }