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 1f72df30b8..130dbfbc3a 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 @@ -131,8 +131,10 @@ class JointTrajectoryController : public controller_interface::ControllerInterfa std::shared_ptr param_listener_; Params params_; + // variables for storing internal data for open-loop control trajectory_msgs::msg::JointTrajectoryPoint last_commanded_state_; rclcpp::Time last_commanded_time_; + /// Specify interpolation method. Default to splines. interpolation_methods::InterpolationMethod interpolation_method_{ interpolation_methods::DEFAULT_INTERPOLATION}; @@ -184,9 +186,9 @@ class JointTrajectoryController : public controller_interface::ControllerInterfa rclcpp::Service::SharedPtr query_state_srv_; - std::shared_ptr traj_external_point_ptr_ = nullptr; + std::shared_ptr current_trajectory_ = nullptr; realtime_tools::RealtimeBuffer> - traj_msg_external_point_ptr_; + new_trajectory_msg_; std::shared_ptr hold_position_msg_ptr_ = nullptr; diff --git a/joint_trajectory_controller/src/cartesian_trajectory_generator.cpp b/joint_trajectory_controller/src/cartesian_trajectory_generator.cpp index a67980b57f..9c73b56568 100644 --- a/joint_trajectory_controller/src/cartesian_trajectory_generator.cpp +++ b/joint_trajectory_controller/src/cartesian_trajectory_generator.cpp @@ -214,7 +214,7 @@ void CartesianTrajectoryGenerator::reference_callback( assign_value_from_input( msg->transforms[0].rotation.z, msg->velocities[0].angular.z, params_.joints[5], 5); - add_new_trajectory_msg(new_traj_msg); + topic_callback(new_traj_msg); } controller_interface::CallbackReturn CartesianTrajectoryGenerator::on_activate( @@ -259,9 +259,8 @@ controller_interface::CallbackReturn CartesianTrajectoryGenerator::on_activate( // } // } - traj_external_point_ptr_ = std::make_shared(); - traj_msg_external_point_ptr_.writeFromNonRT( - std::shared_ptr()); + current_trajectory_ = std::make_shared(); + new_trajectory_msg_.writeFromNonRT(std::shared_ptr()); subscriber_is_active_ = true; diff --git a/joint_trajectory_controller/src/joint_trajectory_controller.cpp b/joint_trajectory_controller/src/joint_trajectory_controller.cpp index 95912fb9e6..8f522b7642 100644 --- a/joint_trajectory_controller/src/joint_trajectory_controller.cpp +++ b/joint_trajectory_controller/src/joint_trajectory_controller.cpp @@ -133,31 +133,33 @@ controller_interface::return_type JointTrajectoryController::update( // don't update goal after we sampled the trajectory to avoid any racecondition const auto active_goal = *rt_active_goal_.readFromRT(); - // 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(); + // Check if a new trajectory message has been received from Non-RT threads + const auto current_trajectory_msg = current_trajectory_->get_trajectory_msg(); + auto new_external_msg = new_trajectory_msg_.readFromRT(); // Discard, if a goal is pending but still not active (somewhere stuck in goal_handle_timer_) if ( - current_external_msg != *new_external_msg && + current_trajectory_msg != *new_external_msg && (*(rt_has_pending_goal_.readFromRT()) && !active_goal) == false) { fill_partial_goal(*new_external_msg); sort_to_local_joint_order(*new_external_msg); // TODO(denis): Add here integration of position and velocity - traj_external_point_ptr_->update(*new_external_msg); + current_trajectory_->update(*new_external_msg); } // current state update state_current_.time_from_start.set__sec(0); if (!read_state_from_state_interfaces(state_current_)) + { return controller_interface::return_type::OK; + } // currently carrying out a trajectory if (has_active_trajectory()) { bool first_sample = false; // if sampling the first time, set the point before you sample - if (!traj_external_point_ptr_->is_sampled_already()) + if (!current_trajectory_->is_sampled_already()) { first_sample = true; @@ -196,24 +198,24 @@ controller_interface::return_type JointTrajectoryController::update( { last_commanded_time_ = time; } - traj_external_point_ptr_->set_point_before_trajectory_msg( + current_trajectory_->set_point_before_trajectory_msg( last_commanded_time_, last_commanded_state_); } else { - traj_external_point_ptr_->set_point_before_trajectory_msg(time, state_current_); + current_trajectory_->set_point_before_trajectory_msg(time, state_current_); } } // find segment for current timestamp TrajectoryPointConstIter start_segment_itr, end_segment_itr; - const bool valid_point = traj_external_point_ptr_->sample( + const bool valid_point = current_trajectory_->sample( time, interpolation_method_, state_desired_, start_segment_itr, end_segment_itr, period, joint_limiter_); if (valid_point) { - const rclcpp::Time traj_start = traj_external_point_ptr_->time_from_start(); + const rclcpp::Time traj_start = current_trajectory_->time_from_start(); // this is the time instance // - started with the first segment: when the first point will be reached (in the future) // - later: when the point of the current segment was reached @@ -225,7 +227,7 @@ controller_interface::return_type JointTrajectoryController::update( bool tolerance_violated_while_moving = false; bool outside_goal_tolerance = false; bool within_goal_time = true; - const bool before_last_point = end_segment_itr != traj_external_point_ptr_->end(); + const bool before_last_point = end_segment_itr != current_trajectory_->end(); // have we reached the end, are not holding position, and is a timeout configured? // Check independently of other tolerances @@ -235,8 +237,8 @@ controller_interface::return_type JointTrajectoryController::update( { RCLCPP_WARN(get_node()->get_logger(), "Aborted due to command timeout"); - traj_msg_external_point_ptr_.reset(); - traj_msg_external_point_ptr_.initRT(set_hold_position()); + new_trajectory_msg_.reset(); + new_trajectory_msg_.initRT(set_hold_position()); } // Check state/goal tolerance @@ -349,8 +351,8 @@ controller_interface::return_type JointTrajectoryController::update( RCLCPP_WARN(get_node()->get_logger(), "Aborted due to state tolerance violation"); - traj_msg_external_point_ptr_.reset(); - traj_msg_external_point_ptr_.initRT(set_hold_position()); + new_trajectory_msg_.reset(); + new_trajectory_msg_.initRT(set_hold_position()); } // check goal tolerance else if (!before_last_point) @@ -368,8 +370,8 @@ controller_interface::return_type JointTrajectoryController::update( RCLCPP_INFO(get_node()->get_logger(), "Goal reached, success!"); - traj_msg_external_point_ptr_.reset(); - traj_msg_external_point_ptr_.initRT(set_success_trajectory_point()); + new_trajectory_msg_.reset(); + new_trajectory_msg_.initRT(set_success_trajectory_point()); } else if (!within_goal_time) { @@ -387,8 +389,8 @@ controller_interface::return_type JointTrajectoryController::update( RCLCPP_WARN(get_node()->get_logger(), error_string.c_str()); - traj_msg_external_point_ptr_.reset(); - traj_msg_external_point_ptr_.initRT(set_hold_position()); + new_trajectory_msg_.reset(); + new_trajectory_msg_.initRT(set_hold_position()); } } } @@ -397,16 +399,16 @@ controller_interface::return_type JointTrajectoryController::update( // we need to ensure that there is no pending goal -> we get a race condition otherwise RCLCPP_ERROR(get_node()->get_logger(), "Holding position due to state tolerance violation"); - traj_msg_external_point_ptr_.reset(); - traj_msg_external_point_ptr_.initRT(set_hold_position()); + new_trajectory_msg_.reset(); + new_trajectory_msg_.initRT(set_hold_position()); } else if ( !before_last_point && !within_goal_time && *(rt_has_pending_goal_.readFromRT()) == false) { RCLCPP_ERROR(get_node()->get_logger(), "Exceeded goal_time_tolerance: holding position..."); - traj_msg_external_point_ptr_.reset(); - traj_msg_external_point_ptr_.initRT(set_hold_position()); + new_trajectory_msg_.reset(); + new_trajectory_msg_.initRT(set_hold_position()); } // else, run another cycle while waiting for outside_goal_tolerance // to be satisfied (will stay in this state until new message arrives) @@ -602,13 +604,13 @@ void JointTrajectoryController::query_state_service( { TrajectoryPointConstIter start_segment_itr, end_segment_itr; const rclcpp::Duration period = rclcpp::Duration::from_seconds(0.01); - response->success = traj_external_point_ptr_->sample( + response->success = current_trajectory_->sample( static_cast(request->time), interpolation_method_, state_requested, start_segment_itr, end_segment_itr, period, joint_limiter_); // If the requested sample time precedes the trajectory finish time respond as failure if (response->success) { - if (end_segment_itr == traj_external_point_ptr_->end()) + if (end_segment_itr == current_trajectory_->end()) { RCLCPP_ERROR(logger, "Requested sample time precedes the current trajectory end time."); response->success = false; @@ -953,9 +955,8 @@ controller_interface::CallbackReturn JointTrajectoryController::on_activate( } } - traj_external_point_ptr_ = std::make_shared(); - traj_msg_external_point_ptr_.writeFromNonRT( - std::shared_ptr()); + current_trajectory_ = std::make_shared(); + new_trajectory_msg_.writeFromNonRT(std::shared_ptr()); subscriber_is_active_ = true; @@ -1054,7 +1055,7 @@ controller_interface::CallbackReturn JointTrajectoryController::on_deactivate( subscriber_is_active_ = false; - traj_external_point_ptr_.reset(); + current_trajectory_.reset(); return CallbackReturn::SUCCESS; } @@ -1088,7 +1089,7 @@ bool JointTrajectoryController::reset() } } - traj_external_point_ptr_.reset(); + current_trajectory_.reset(); return true; } @@ -1315,15 +1316,6 @@ void JointTrajectoryController::compute_error_for_joint( // error defined as the difference between current and desired if (joints_angle_wraparound_[index]) { - RCLCPP_WARN( - get_node()->get_logger(), "Calculating angular distance between sizes: %zu and desired: %zu.", - current.positions.size(), desired.positions.size()); - RCLCPP_WARN( - get_node()->get_logger(), "Calculating angular distance between current: %.5f.", - current.positions[index]); - RCLCPP_WARN( - get_node()->get_logger(), "Calculating angular distance between desired: %.5f.", - desired.positions[index]); // if desired, the shortest_angular_distance is calculated, i.e., the error is // normalized between -piget_logger(), "Calculating for index: %zu.", index); - RCLCPP_WARN( - get_node()->get_logger(), "Calculating angular distance between sizes: %zu and desired: %zu.", - current.positions.size(), desired.positions.size()); - RCLCPP_WARN( - get_node()->get_logger(), "Calculating angular distance between current: %.5f.", - current.positions[index]); - RCLCPP_WARN( - get_node()->get_logger(), "Calculating angular distance between desired: %.5f.", - desired.positions[index]); error.positions[index] = desired.positions[index] - current.positions[index]; } if ( @@ -1488,6 +1470,7 @@ bool JointTrajectoryController::validate_trajectory_point_field( bool JointTrajectoryController::validate_trajectory_msg( const trajectory_msgs::msg::JointTrajectory & trajectory) const { + // CHECK: Partial joint goals // If partial joints goals are not allowed, goal should specify all controller joints if (!params_.allow_partial_joints_goal) { @@ -1500,33 +1483,21 @@ bool JointTrajectoryController::validate_trajectory_msg( } } + // CHECK: if joint names are provided if (trajectory.joint_names.empty()) { RCLCPP_ERROR(get_node()->get_logger(), "Empty joint names on incoming trajectory."); return false; } - const auto trajectory_start_time = static_cast(trajectory.header.stamp); - // If the starting time it set to 0.0, it means the controller should start it now. - // Otherwise we check if the trajectory ends before the current time, - // in which case it can be ignored. - if (trajectory_start_time.seconds() != 0.0) + // CHECK: if provided trajectory has points + if (trajectory.points.empty()) { - auto trajectory_end_time = trajectory_start_time; - for (const auto & p : trajectory.points) - { - trajectory_end_time += p.time_from_start; - } - if (trajectory_end_time < get_node()->now()) - { - RCLCPP_ERROR( - get_node()->get_logger(), - "Received trajectory with non-zero start time (%f) that ends in the past (%f)", - trajectory_start_time.seconds(), trajectory_end_time.seconds()); - return false; - } + RCLCPP_ERROR(get_node()->get_logger(), "Empty trajectory received."); + return false; } + // CHECK: If joint names are matching the joints defined for the controller for (size_t i = 0; i < trajectory.joint_names.size(); ++i) { const std::string & incoming_joint_name = trajectory.joint_names[i]; @@ -1541,12 +1512,7 @@ bool JointTrajectoryController::validate_trajectory_msg( } } - if (trajectory.points.empty()) - { - RCLCPP_ERROR(get_node()->get_logger(), "Empty trajectory received."); - return false; - } - + // CHECK: if trajectory ends with non-zero velocity (when option is disabled) if (!params_.allow_nonzero_velocity_at_trajectory_end) { for (size_t i = 0; i < trajectory.points.back().velocities.size(); ++i) @@ -1562,9 +1528,32 @@ bool JointTrajectoryController::validate_trajectory_msg( } } + // CHECK: if trajectory end time is in the past (if start time defined) + const auto trajectory_start_time = static_cast(trajectory.header.stamp); + // If the starting time it set to 0.0, it means the controller should start it now. + // Otherwise we check if the trajectory ends before the current time, + // in which case it can be ignored. + if (trajectory_start_time.seconds() != 0.0) + { + auto trajectory_end_time = trajectory_start_time; + for (const auto & p : trajectory.points) + { + trajectory_end_time += p.time_from_start; + } + if (trajectory_end_time < get_node()->now()) + { + RCLCPP_ERROR( + get_node()->get_logger(), + "Received trajectory with non-zero start time (%f) that ends in the past (%f)", + trajectory_start_time.seconds(), trajectory_end_time.seconds()); + return false; + } + } + rclcpp::Duration previous_traj_time(0ms); for (size_t i = 0; i < trajectory.points.size(); ++i) { + // CHECK: if time of points in the trajectory is monotonous if ((i > 0) && (rclcpp::Duration(trajectory.points[i].time_from_start) <= previous_traj_time)) { RCLCPP_ERROR( @@ -1578,6 +1567,8 @@ bool JointTrajectoryController::validate_trajectory_msg( const size_t joint_count = trajectory.joint_names.size(); const auto & points = trajectory.points; + + // CHECK: if all required data are provided in the trajectory // This currently supports only position, velocity and acceleration inputs if (params_.allow_integration_in_goal_trajectories) { @@ -1620,7 +1611,7 @@ bool JointTrajectoryController::validate_trajectory_msg( void JointTrajectoryController::add_new_trajectory_msg( const std::shared_ptr & traj_msg) { - traj_msg_external_point_ptr_.writeFromNonRT(traj_msg); + new_trajectory_msg_.writeFromNonRT(traj_msg); } void JointTrajectoryController::preempt_active_goal() @@ -1653,7 +1644,7 @@ JointTrajectoryController::set_success_trajectory_point() { // set last command to be repeated at success, no matter if it has nonzero velocity or // acceleration - hold_position_msg_ptr_->points[0] = traj_external_point_ptr_->get_trajectory_msg()->points.back(); + hold_position_msg_ptr_->points[0] = current_trajectory_->get_trajectory_msg()->points.back(); hold_position_msg_ptr_->points[0].time_from_start = rclcpp::Duration(0, 0); // set flag, otherwise tolerances will be checked with success_trajectory_point too @@ -1706,7 +1697,7 @@ void JointTrajectoryController::resize_joint_trajectory_point_command( bool JointTrajectoryController::has_active_trajectory() const { - return traj_external_point_ptr_ != nullptr && traj_external_point_ptr_->has_trajectory_msg(); + return current_trajectory_ != nullptr && current_trajectory_->has_trajectory_msg(); } void JointTrajectoryController::update_pids()