diff --git a/joint_trajectory_controller/include/joint_trajectory_controller/joint_trajectory_controller.hpp b/joint_trajectory_controller/include/joint_trajectory_controller/joint_trajectory_controller.hpp index 1f5a75b4a4..67ccbcd925 100644 --- a/joint_trajectory_controller/include/joint_trajectory_controller/joint_trajectory_controller.hpp +++ b/joint_trajectory_controller/include/joint_trajectory_controller/joint_trajectory_controller.hpp @@ -22,6 +22,7 @@ #include #include +// TODO: try minimizing the amount of includes #include "control_msgs/action/follow_joint_trajectory.hpp" #include "control_msgs/msg/joint_trajectory_controller_state.hpp" #include "control_toolbox/pid.hpp" @@ -45,15 +46,12 @@ using namespace std::chrono_literals; // NOLINT +// TODO check if this is needed at all namespace rclcpp_action { template class ServerGoalHandle; } // namespace rclcpp_action -namespace rclcpp_lifecycle -{ -class State; -} // namespace rclcpp_lifecycle namespace joint_trajectory_controller { @@ -163,6 +161,7 @@ class JointTrajectoryController : public controller_interface::ControllerInterfa rclcpp::Subscription::SharedPtr joint_command_subscriber_ = nullptr; + // TODO clean this mess std::shared_ptr * traj_point_active_ptr_ = nullptr; std::shared_ptr traj_external_point_ptr_ = nullptr; std::shared_ptr traj_home_point_ptr_ = nullptr; @@ -170,10 +169,6 @@ class JointTrajectoryController : public controller_interface::ControllerInterfa realtime_tools::RealtimeBuffer> traj_msg_external_point_ptr_; - // The controller should be in halted state after creation otherwise memory corruption - // TODO(anyone): Is the variable relevant, since we are using lifecycle? - bool is_halted_ = true; - using ControllerStateMsg = control_msgs::msg::JointTrajectoryControllerState; using StatePublisher = realtime_tools::RealtimePublisher; using StatePublisherPtr = std::unique_ptr; @@ -205,6 +200,7 @@ class JointTrajectoryController : public controller_interface::ControllerInterfa void feedback_setup_callback( std::shared_ptr> goal_handle); + // TODO try refactoring these into trajectory_operations.cpp or into trajectory.hpp directly // fill trajectory_msg so it matches joints controlled by this controller // positions set to current position, velocities, accelerations and efforts to 0.0 JOINT_TRAJECTORY_CONTROLLER_PUBLIC @@ -240,7 +236,7 @@ class JointTrajectoryController : public controller_interface::ControllerInterfa const JointTrajectoryPoint & desired_state, const JointTrajectoryPoint & current_state, const JointTrajectoryPoint & state_error); - void read_state_from_hardware(JointTrajectoryPoint & state); + void read_state_from_state_interfaces(JointTrajectoryPoint & state); bool read_state_from_command_interfaces(JointTrajectoryPoint & state); diff --git a/joint_trajectory_controller/src/joint_trajectory_controller.cpp b/joint_trajectory_controller/src/joint_trajectory_controller.cpp index 0af506b83e..d78f035b66 100644 --- a/joint_trajectory_controller/src/joint_trajectory_controller.cpp +++ b/joint_trajectory_controller/src/joint_trajectory_controller.cpp @@ -23,6 +23,7 @@ #include #include +// TODO this is way too much #include "angles/angles.h" #include "builtin_interfaces/msg/duration.hpp" #include "builtin_interfaces/msg/time.hpp" @@ -45,8 +46,7 @@ namespace joint_trajectory_controller { -JointTrajectoryController::JointTrajectoryController() -: controller_interface::ControllerInterface(), joint_names_({}) +JointTrajectoryController::JointTrajectoryController() : controller_interface::ControllerInterface() { } @@ -116,6 +116,7 @@ controller_interface::return_type JointTrajectoryController::update( return controller_interface::return_type::OK; } + // TODO turn this into a proper function & unit test plz auto compute_error_for_joint = [&]( JointTrajectoryPoint & error, int index, const JointTrajectoryPoint & current, @@ -133,6 +134,7 @@ controller_interface::return_type JointTrajectoryController::update( } }; + // TODO if "RealtimeSubscriber" existed with an API to check for new message & get message that'd make this very readable // Check if a new external message has been received from nonRT threads auto current_external_msg = traj_external_point_ptr_->get_trajectory_msg(); auto new_external_msg = traj_msg_external_point_ptr_.readFromRT(); @@ -144,10 +146,15 @@ controller_interface::return_type JointTrajectoryController::update( traj_external_point_ptr_->update(*new_external_msg); } + // TODO these should be class members and reserved in the configure method + // they may be resized here but if they were reserved large enough, it's ok + // it should also be cleaned at the end of update() + // perhaps this could be a class of it's own JointTrajectoryPoint state_current, state_desired, state_error; const auto joint_num = joint_names_.size(); resize_joint_trajectory_point(state_current, joint_num); + // TODO please convert into member function // TODO(anyone): can I here also use const on joint_interface since the reference_wrapper is not // changed, but its value only? auto assign_interface_from_point = @@ -159,8 +166,8 @@ controller_interface::return_type JointTrajectoryController::update( }; // current state update - state_current.time_from_start.set__sec(0); - read_state_from_hardware(state_current); + state_current.time_from_start.sec = 0; + read_state_from_state_interfaces(state_current); // currently carrying out a trajectory if (traj_point_active_ptr_ && (*traj_point_active_ptr_)->has_trajectory_msg()) @@ -181,6 +188,7 @@ controller_interface::return_type JointTrajectoryController::update( // find segment for current timestamp TrajectoryPointConstIter start_segment_itr, end_segment_itr; + // trajectory interpolation is always open-loop const bool valid_point = (*traj_point_active_ptr_)->sample(time, state_desired, start_segment_itr, end_segment_itr); @@ -335,7 +343,7 @@ controller_interface::return_type JointTrajectoryController::update( return controller_interface::return_type::OK; } -void JointTrajectoryController::read_state_from_hardware(JointTrajectoryPoint & state) +void JointTrajectoryController::read_state_from_state_interfaces(JointTrajectoryPoint & state) { const auto joint_num = joint_names_.size(); auto assign_point_from_interface = @@ -809,7 +817,7 @@ controller_interface::CallbackReturn JointTrajectoryController::on_activate( // Initialize current state storage if hardware state has tracking offset resize_joint_trajectory_point(last_commanded_state_, joint_names_.size()); - read_state_from_hardware(last_commanded_state_); + read_state_from_state_interfaces(last_commanded_state_); // Handle restart of controller by reading last_commanded_state_ from commands is // those are not nan trajectory_msgs::msg::JointTrajectoryPoint state;