small fix

lucabeber · Feb 22, 2024 · fe22d3b · fe22d3b
1 parent c7f17fe
commit fe22d3b
Show file tree

Hide file tree

Showing 2 changed files with 5 additions and 1 deletion.
diff --git 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
@@ -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;
 }