diff --git a/examples/example_movej.py b/examples/example_movej.py index 2376212e5..d6a8ee071 100644 --- a/examples/example_movej.py +++ b/examples/example_movej.py @@ -29,7 +29,8 @@ import rclpy from builtin_interfaces.msg import Duration -from control_msgs.action import JointTrajectory +from control_msgs.action import FollowJointTrajectory +from control_msgs.msg import JointTolerance from rclpy.action import ActionClient from rclpy.node import Node @@ -100,7 +101,7 @@ def init_robot(self): self.jtc_action_client = waitForAction( self.node, "/passthrough_trajectory_controller/forward_joint_trajectory", - JointTrajectory, + FollowJointTrajectory, ) time.sleep(2) @@ -127,14 +128,23 @@ def send_trajectory(self, waypts, time_vec): point.time_from_start = time_vec[i] joint_trajectory.points.append(point) + tolerances = [JointTolerance(position=0.001) for i in range(6)] + time_tolerance = Duration() + time_tolerance.sec = 1 # Sending trajectory goal goal_response = self.call_action( - self.jtc_action_client, JointTrajectory.Goal(trajectory=joint_trajectory) + self.jtc_action_client, + FollowJointTrajectory.Goal( + trajectory=joint_trajectory, + goal_tolerance=tolerances, + goal_time_tolerance=time_tolerance, + ), ) if goal_response.accepted is False: raise Exception("trajectory was not accepted") # Verify execution + result = self.get_result(self.jtc_action_client, goal_response) return result @@ -217,7 +227,7 @@ def load_passthrough_controller(self): self.jtc_action_client = waitForAction( self.node, "/passthrough_trajectory_controller/forward_joint_trajectory", - JointTrajectory, + FollowJointTrajectory, ) time.sleep(2) @@ -242,9 +252,9 @@ def switch_controller(self, active, inactive): # The following list are arbitrary joint positions, change according to your own needs waypts = [ - [-1, -2.5998, -1.004, -2.676, -0.992, -1.5406], - [-0.1, -2.6998, -1.104, -2.676, -0.992, -1.5406], - [-1, -2.5998, -1.004, -2.676, -0.992, -1.5406], + [-1.58, -1.692, -1.4311, -0.0174, 1.5882, 0.0349], + [-3, -1.692, -1.4311, -0.0174, 1.5882, 0.0349], + [-1.58, -1.692, -1.4311, -0.0174, 1.5882, 0.0349], ] time_vec = [Duration(sec=4, nanosec=0), Duration(sec=8, nanosec=0), Duration(sec=12, nanosec=0)] diff --git a/examples/example_spline.py b/examples/example_spline.py index 059333da7..e00c4bb98 100644 --- a/examples/example_spline.py +++ b/examples/example_spline.py @@ -29,7 +29,7 @@ import rclpy from builtin_interfaces.msg import Duration -from control_msgs.action import JointTrajectory +from control_msgs.action import FollowJointTrajectory from rclpy.action import ActionClient from rclpy.node import Node @@ -100,7 +100,7 @@ def init_robot(self): self.jtc_action_client = waitForAction( self.node, "/passthrough_trajectory_controller/forward_joint_trajectory", - JointTrajectory, + FollowJointTrajectory, ) time.sleep(2) @@ -131,7 +131,7 @@ def send_trajectory(self, waypts, time_vec, vels, accels): # Sending trajectory goal goal_response = self.call_action( - self.jtc_action_client, JointTrajectory.Goal(trajectory=joint_trajectory) + self.jtc_action_client, FollowJointTrajectory.Goal(trajectory=joint_trajectory) ) if goal_response.accepted is False: raise Exception("trajectory was not accepted") @@ -219,7 +219,7 @@ def load_passthrough_controller(self): self.jtc_action_client = waitForAction( self.node, "/passthrough_trajectory_controller/forward_joint_trajectory", - JointTrajectory, + FollowJointTrajectory, ) time.sleep(2) diff --git a/examples/examples.py b/examples/examples.py index 79364c97a..9d5f4a49d 100644 --- a/examples/examples.py +++ b/examples/examples.py @@ -29,7 +29,7 @@ import rclpy from builtin_interfaces.msg import Duration -from control_msgs.action import JointTrajectory +from control_msgs.action import FollowJointTrajectory from rclpy.action import ActionClient from rclpy.node import Node @@ -123,7 +123,7 @@ def send_trajectory(self, waypts, time_vec): # Sending trajectory goal goal_response = self.call_action( - self.jtc_action_client, JointTrajectory.Goal(trajectory=joint_trajectory) + self.jtc_action_client, FollowJointTrajectory.Goal(trajectory=joint_trajectory) ) if goal_response.accepted is False: raise Exception("trajectory was not accepted") @@ -211,7 +211,7 @@ def load_passthrough_controller(self): self.jtc_action_client = waitForAction( self.node, "/passthrough_trajectory_controller/forward_joint_trajectory", - JointTrajectory, + FollowJointTrajectory, ) time.sleep(2) diff --git a/examples/load_switch_controller_example.py b/examples/load_switch_controller_example.py index c5a5a6ceb..aa0fd8b52 100644 --- a/examples/load_switch_controller_example.py +++ b/examples/load_switch_controller_example.py @@ -29,7 +29,7 @@ import rclpy from builtin_interfaces.msg import Duration -from control_msgs.action import JointTrajectory +from control_msgs.action import FollowJointTrajectory from rclpy.action import ActionClient from rclpy.node import Node @@ -123,7 +123,7 @@ def send_trajectory(self, waypts, time_vec): # Sending trajectory goal goal_response = self.call_action( - self.jtc_action_client, JointTrajectory.Goal(trajectory=joint_trajectory) + self.jtc_action_client, FollowJointTrajectory.Goal(trajectory=joint_trajectory) ) if goal_response.accepted is False: raise Exception("trajectory was not accepted") @@ -211,7 +211,7 @@ def load_passthrough_controller(self): self.jtc_action_client = waitForAction( self.node, "/passthrough_trajectory_controller/forward_joint_trajectory", - JointTrajectory, + FollowJointTrajectory, ) time.sleep(2) diff --git a/ur_controllers/include/ur_controllers/passthrough_trajectory_controller.hpp b/ur_controllers/include/ur_controllers/passthrough_trajectory_controller.hpp index c6cf9f269..468201f11 100644 --- a/ur_controllers/include/ur_controllers/passthrough_trajectory_controller.hpp +++ b/ur_controllers/include/ur_controllers/passthrough_trajectory_controller.hpp @@ -51,6 +51,8 @@ #include "rclcpp_action/create_server.hpp" #include "rclcpp/rclcpp.hpp" #include "rclcpp_action/server_goal_handle.hpp" +#include "rclcpp/time.hpp" +#include "rclcpp/duration.hpp" #include "trajectory_msgs/msg/joint_trajectory.hpp" #include "trajectory_msgs/msg/joint_trajectory_point.hpp" @@ -63,15 +65,36 @@ namespace ur_controllers { enum CommandInterfaces { + /* The PASSTHROUGH_TRAJECTORY_TRANSFER_STATE value is used to keep track of which stage the transfer is in. + 0.0: No trajectory to forward, the controller is idling and ready to receive a new trajectory. + 1.0: The controller has received and accepted a new trajecotry. When the state is 1.0, the controller will write a + point to the hardware interface. + 2.0: The hardware interface will read the point written from the controller. The state will switch between 1.0 + and 2.0 until all points have been read by the hardware interface. + 3.0: The hardware interface has read all the points, and will now write all the points to the robot driver. + 4.0: The robot is moving through the trajectory. + 5.0: The robot finished executing the trajectory. */ PASSTHROUGH_TRAJECTORY_TRANSFER_STATE = 0, - PASSTHROUGH_POINT_WRITTEN = 1, - PASSTHROUGH_TRAJECTORY_NUMBER_OF_POINTS = 2, - PASSTHROUGH_TRAJECTORY_CANCEL = 3, - PASSTHROUGH_TRAJECTORY_DIMENSIONS = 4, - PASSTHROUGH_TRAJECTORY_POSITIONS_ = 5, - PASSTHROUGH_TRAJECTORY_VELOCITIES_ = 11, - PASSTHROUGH_TRAJECTORY_ACCELERATIONS_ = 17, - PASSTHROUGH_TRAJECTORY_TIME_FROM_START = 23 + /* The PASSTHROUGH_TRAJECTORY_CANCEL value is used to indicate whether the trajectory has been cancelled from the + * hardware interface.*/ + PASSTHROUGH_TRAJECTORY_CANCEL = 1, + /* The PASSTHROUGH_TRAJECTORY_DIMENSIONS is used to indicate what combination of joint positions, velocities and + * accelerations the trajectory consists of, see check_dimensions() for a description of what the values mean. */ + PASSTHROUGH_TRAJECTORY_DIMENSIONS = 2, + /* Arrays to hold the values of a point. */ + PASSTHROUGH_TRAJECTORY_POSITIONS_ = 3, + PASSTHROUGH_TRAJECTORY_VELOCITIES_ = 9, + PASSTHROUGH_TRAJECTORY_ACCELERATIONS_ = 15, + PASSTHROUGH_TRAJECTORY_TIME_FROM_START = 21 +}; + +enum StateInterfaces +{ + /* State interface 0 - 17 are joint state interfaces*/ + + SPEED_SCALING_FACTOR = 18, + NUMBER_OF_JOINTS = 19, + CONTROLLER_RUNNING = 20 }; class PassthroughTrajectoryController : public controller_interface::ControllerInterface @@ -93,32 +116,64 @@ class PassthroughTrajectoryController : public controller_interface::ControllerI controller_interface::return_type update(const rclcpp::Time& time, const rclcpp::Duration& period) override; private: + /* Start an action server with an action called: /passthrough_trajectory_controller/forward_joint_trajectory. */ void start_action_server(void); + void end_goal(); + + bool check_goal_tolerance(); + std::shared_ptr passthrough_param_listener_; passthrough_trajectory_controller::Params passthrough_params_; - rclcpp_action::Server::SharedPtr send_trajectory_action_server_; + rclcpp_action::Server::SharedPtr send_trajectory_action_server_; - rclcpp_action::GoalResponse goal_received_callback( - const rclcpp_action::GoalUUID& uuid, std::shared_ptr goal); + rclcpp_action::GoalResponse + goal_received_callback(const rclcpp_action::GoalUUID& uuid, + std::shared_ptr goal); rclcpp_action::CancelResponse goal_cancelled_callback( - const std::shared_ptr> goal_handle); + const std::shared_ptr> goal_handle); void goal_accepted_callback( - const std::shared_ptr> goal_handle); + const std::shared_ptr> goal_handle); + + void execute( + const std::shared_ptr> goal_handle); + + std::vector joint_names_; + std::vector state_interface_types_; + + std::vector> joint_position_state_interface_; + std::vector> joint_velocity_state_interface_; + + /* + If there are values in the velocities and accelerations vectors, they should have as many elements as there are + joints, and all be the same size. + The return value indicates what combination of joint positions, velocities and accelerations is present; + 0: The trajectory is invalid, and will be rejected. + 1: Only positions are defined for the trajectory. + 2: Positions and velocities are defined for the trajectory. + 3: Positions and accelerations are defined for the trajectory. + 4: Both positions, velocities and accelerations are defined for the trajectory. + */ + int check_dimensions(std::shared_ptr goal); - void - execute(const std::shared_ptr> goal_handle); - int check_dimensions(std::shared_ptr goal); trajectory_msgs::msg::JointTrajectory active_joint_traj_; + std::vector path_tolerance_; + std::vector goal_tolerance_; + rclcpp::Duration goal_time_tolerance_ = rclcpp::Duration::from_nanoseconds(0); + uint32_t current_point_; bool trajectory_active_; - uint64_t active_trajectory_elapsed_time_; - uint64_t max_trajectory_time_; + uint64_t period_ns; + uint64_t last_time_ns; + uint64_t now_ns; + double active_trajectory_elapsed_time_; + double max_trajectory_time_; double scaling_factor_; - std::shared_ptr> current_handle; + uint32_t number_of_joints_; + std::shared_ptr> active_goal_; }; } // namespace ur_controllers #endif // UR_CONTROLLERS__PASSTHROUGH_TRAJECTORY_CONTROLLER_HPP_ diff --git a/ur_controllers/src/passthrough_trajectory_controller.cpp b/ur_controllers/src/passthrough_trajectory_controller.cpp index 81935f287..092d6aa5e 100644 --- a/ur_controllers/src/passthrough_trajectory_controller.cpp +++ b/ur_controllers/src/passthrough_trajectory_controller.cpp @@ -49,7 +49,8 @@ controller_interface::CallbackReturn PassthroughTrajectoryController::on_init() passthrough_param_listener_ = std::make_shared(get_node()); passthrough_params_ = passthrough_param_listener_->get_params(); current_point_ = 0; - + joint_names_ = auto_declare>("joints", joint_names_); + state_interface_types_ = auto_declare("state_interfaces", state_interface_types_); return controller_interface::CallbackReturn::SUCCESS; } @@ -64,7 +65,7 @@ PassthroughTrajectoryController::on_configure(const rclcpp_lifecycle::State& pre void PassthroughTrajectoryController::start_action_server(void) { - send_trajectory_action_server_ = rclcpp_action::create_server( + send_trajectory_action_server_ = rclcpp_action::create_server( get_node(), std::string(get_node()->get_name()) + "/forward_joint_trajectory", std::bind(&PassthroughTrajectoryController::goal_received_callback, this, std::placeholders::_1, std::placeholders::_2), @@ -76,10 +77,18 @@ void PassthroughTrajectoryController::start_action_server(void) controller_interface::InterfaceConfiguration PassthroughTrajectoryController::state_interface_configuration() const { controller_interface::InterfaceConfiguration conf; - conf.type = controller_interface::interface_configuration_type::INDIVIDUAL; + const std::string tf_prefix = passthrough_params_.tf_prefix; - conf.names.push_back(passthrough_params_.speed_scaling_interface_name); + conf.names.reserve(joint_names_.size() * state_interface_types_.size()); + for (const auto& joint_name : joint_names_) { + for (const auto& interface_type : state_interface_types_) { + conf.names.emplace_back(joint_name + "/" + interface_type); + } + } + conf.names.emplace_back(passthrough_params_.speed_scaling_interface_name); + conf.names.emplace_back(tf_prefix + "passthrough_controller/number_of_joints"); + conf.names.emplace_back(tf_prefix + "passthrough_controller/running"); return conf; } @@ -93,10 +102,6 @@ controller_interface::InterfaceConfiguration PassthroughTrajectoryController::co config.names.emplace_back(tf_prefix + "passthrough_controller/passthrough_trajectory_transfer_state"); - config.names.emplace_back(tf_prefix + "passthrough_controller/passthrough_point_written"); - - config.names.emplace_back(tf_prefix + "passthrough_controller/passthrough_trajectory_number_of_points"); - config.names.emplace_back(tf_prefix + "passthrough_controller/passthrough_trajectory_cancel"); config.names.emplace_back(tf_prefix + "passthrough_controller/passthrough_trajectory_dimensions"); @@ -121,53 +126,73 @@ controller_interface::InterfaceConfiguration PassthroughTrajectoryController::co controller_interface::CallbackReturn PassthroughTrajectoryController::on_activate(const rclcpp_lifecycle::State& state) { + number_of_joints_ = joint_names_.size(); + // clear out vectors in case of restart + joint_position_state_interface_.clear(); + joint_velocity_state_interface_.clear(); + + for (uint32_t i = 0; i < state_interface_types_.size() * joint_names_.size(); i++) { + if (state_interfaces_[i].get_interface_name() == "position") { + joint_position_state_interface_.emplace_back(state_interfaces_[i]); + } else if (state_interfaces_[i].get_interface_name() == "velocity") { + joint_velocity_state_interface_.emplace_back(state_interfaces_[i]); + } + } + return ControllerInterface::on_activate(state); } -controller_interface::return_type PassthroughTrajectoryController::update(const rclcpp::Time& /*time*/, - const rclcpp::Duration& period) +controller_interface::return_type PassthroughTrajectoryController::update(const rclcpp::Time& time, + const rclcpp::Duration& /* period */) { - // static int delay = 1500; - // if (!delay) { - // std::cout << "Speed scaling : " << state_interfaces_[0].get_value() << std::endl; - // std::cout << "Trajectory executing : " << trajectory_active_ << std::endl; - // delay = 1500; - // } - - // if (delay) - // delay--; - - if (command_interfaces_[CommandInterfaces::PASSTHROUGH_TRAJECTORY_TRANSFER_STATE].get_value() == 3.0) { - scaling_factor_ = state_interfaces_[0].get_value(); - active_trajectory_elapsed_time_ += - static_cast(scaling_factor_ * ((period.seconds() * pow(10, 9)) + period.nanoseconds())); - - if (active_trajectory_elapsed_time_ > max_trajectory_time_ && trajectory_active_) { + static bool firstpass = true; + if (firstpass) { + now_ns = time.nanoseconds(); + firstpass = false; + } else { + last_time_ns = now_ns; + now_ns = time.nanoseconds(); + period_ns = now_ns - last_time_ns; + } + /* Keep track of how long the trajectory has been executing, if it takes too long, send a warning. */ + if (command_interfaces_[CommandInterfaces::PASSTHROUGH_TRAJECTORY_TRANSFER_STATE].get_value() == 4.0) { + scaling_factor_ = state_interfaces_[StateInterfaces::SPEED_SCALING_FACTOR].get_value(); + active_trajectory_elapsed_time_ += static_cast(scaling_factor_ * (period_ns / pow(10, 9))); + if (active_trajectory_elapsed_time_ > (max_trajectory_time_ + goal_time_tolerance_.seconds()) && + trajectory_active_) { RCLCPP_WARN(get_node()->get_logger(), "Trajectory should be finished by now. You may want to cancel this goal, " "if it is not."); trajectory_active_ = false; } } + return controller_interface::return_type::OK; } rclcpp_action::GoalResponse PassthroughTrajectoryController::goal_received_callback( - const rclcpp_action::GoalUUID& uuid, std::shared_ptr goal) + const rclcpp_action::GoalUUID& /*uuid*/, + std::shared_ptr goal) { - RCLCPP_INFO(get_node()->get_logger(), "Received new trajectory to forward to robot"); + RCLCPP_INFO(get_node()->get_logger(), "Received new trajectory."); // Precondition: Running controller if (get_state().id() == lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE) { RCLCPP_ERROR(get_node()->get_logger(), "Can't accept new trajectories. Controller is not running."); return rclcpp_action::GoalResponse::REJECT; } + if (state_interfaces_[StateInterfaces::CONTROLLER_RUNNING].get_value() != 1.0) { + RCLCPP_ERROR(get_node()->get_logger(), "Trajectory rejected, controller not running in hardware interface."); + return rclcpp_action::GoalResponse::REJECT; + } + if (command_interfaces_[CommandInterfaces::PASSTHROUGH_TRAJECTORY_TRANSFER_STATE].get_value() != 0.0) { RCLCPP_ERROR(get_node()->get_logger(), "Can't accept new trajectory. A trajectory is already executing."); return rclcpp_action::GoalResponse::REJECT; } + // Check dimensions of the trajectory if (check_dimensions(goal) == 0) { - RCLCPP_ERROR(get_node()->get_logger(), "Trajectory rejected, dimensions of trajectory are wrong."); + RCLCPP_ERROR(get_node()->get_logger(), "Trajectory rejected, dimensions of trajectory are incorrect."); return rclcpp_action::GoalResponse::REJECT; } else { command_interfaces_[CommandInterfaces::PASSTHROUGH_TRAJECTORY_DIMENSIONS].set_value( @@ -176,32 +201,39 @@ rclcpp_action::GoalResponse PassthroughTrajectoryController::goal_received_callb return rclcpp_action::GoalResponse::ACCEPT_AND_EXECUTE; } -// This function should be told how many robot joints there are. Not just run with 6 or 0. + int PassthroughTrajectoryController::check_dimensions( - std::shared_ptr goal) + std::shared_ptr goal) { for (uint32_t i = 0; i < goal->trajectory.points.size(); i++) { std::string msg; - if (goal->trajectory.points[i].positions.size() != 6) { - RCLCPP_ERROR(get_node()->get_logger(), "Can't accept new trajectory. All trajectory points must have positions " - "for all joints of the robot. (6 joint positions per point)"); + if (goal->trajectory.points[i].positions.size() != number_of_joints_) { + msg = "Can't accept new trajectory. All trajectory points must have positions for all joints of the robot. (" + + std::to_string(number_of_joints_) + " joint positions per point)"; + RCLCPP_ERROR(get_node()->get_logger(), msg.c_str()); msg = "Point nr " + std::to_string(i + 1) + " has: " + std::to_string(goal->trajectory.points[i].positions.size()) + " positions."; RCLCPP_ERROR(get_node()->get_logger(), msg.c_str()); return 0; } - if (goal->trajectory.points[i].velocities.size() != 0 && goal->trajectory.points[i].velocities.size() != 6) { - RCLCPP_ERROR(get_node()->get_logger(), "Can't accept new trajectory. All trajectory points must have velocities " - "for all joints of the robot. (6 joint velocities per point)"); + if ((goal->trajectory.points[i].velocities.size() != 0 && + goal->trajectory.points[i].velocities.size() != number_of_joints_) || + goal->trajectory.points[i].velocities.size() != goal->trajectory.points[0].velocities.size()) { + msg = "Can't accept new trajectory. All trajectory points must have velocities for all joints of the robot. (" + + std::to_string(number_of_joints_) + " joint velocities per point)"; + RCLCPP_ERROR(get_node()->get_logger(), msg.c_str()); msg = "Point nr " + std::to_string(i + 1) + " has: " + std::to_string(goal->trajectory.points[i].velocities.size()) + " velocities."; RCLCPP_ERROR(get_node()->get_logger(), msg.c_str()); return 0; } - if (goal->trajectory.points[i].accelerations.size() != 0 && goal->trajectory.points[i].accelerations.size() != 6) { - RCLCPP_ERROR(get_node()->get_logger(), "Can't accept new trajectory. All trajectory points must have " - "accelerations " - "for all joints of the robot. (6 joint accelerations per point)"); + if ((goal->trajectory.points[i].accelerations.size() != 0 && + goal->trajectory.points[i].accelerations.size() != number_of_joints_) || + goal->trajectory.points[i].accelerations.size() != goal->trajectory.points[0].accelerations.size()) { + msg = "Can't accept new trajectory. All trajectory points must have accelerations for all joints of the robot. " + "(" + + std::to_string(number_of_joints_) + " joint accelerations per point)"; + RCLCPP_ERROR(get_node()->get_logger(), msg.c_str()); msg = "Point nr " + std::to_string(i + 1) + " has: " + std::to_string(goal->trajectory.points[i].accelerations.size()) + " accelerations."; RCLCPP_ERROR(get_node()->get_logger(), msg.c_str()); @@ -225,24 +257,19 @@ int PassthroughTrajectoryController::check_dimensions( } } +// Called when the action is cancelled by the action client. rclcpp_action::CancelResponse PassthroughTrajectoryController::goal_cancelled_callback( - const std::shared_ptr> goal_handle) + const std::shared_ptr< + rclcpp_action::ServerGoalHandle> /* goal_handle */) { RCLCPP_INFO(get_node()->get_logger(), "Cancelling active trajectory because cancel callback received."); - command_interfaces_[CommandInterfaces::PASSTHROUGH_TRAJECTORY_TRANSFER_STATE].set_value(0.0); - current_point_ = 0; - command_interfaces_[CommandInterfaces::PASSTHROUGH_TRAJECTORY_CANCEL].set_value(1.0); - std::shared_ptr result = - std::make_shared(); - goal_handle->canceled(result); - trajectory_active_ = false; return rclcpp_action::CancelResponse::ACCEPT; } +// Action goal was accepted, initialise values for a new trajectory. void PassthroughTrajectoryController::goal_accepted_callback( - const std::shared_ptr> goal_handle) + const std::shared_ptr> goal_handle) { - current_handle = goal_handle; RCLCPP_INFO(get_node()->get_logger(), "Accepted new trajectory."); trajectory_active_ = true; active_trajectory_elapsed_time_ = 0; @@ -251,53 +278,50 @@ void PassthroughTrajectoryController::goal_accepted_callback( command_interfaces_[CommandInterfaces::PASSTHROUGH_TRAJECTORY_CANCEL].set_value(0.0); active_joint_traj_ = goal_handle->get_goal()->trajectory; + goal_tolerance_ = goal_handle->get_goal()->goal_tolerance; + path_tolerance_ = goal_handle->get_goal()->path_tolerance; + goal_time_tolerance_ = goal_handle->get_goal()->goal_time_tolerance; + active_goal_ = goal_handle; - max_trajectory_time_ = (active_joint_traj_.points.back().time_from_start.sec * pow(10, 9)) + - active_joint_traj_.points.back().time_from_start.nanosec; - - command_interfaces_[CommandInterfaces::PASSTHROUGH_TRAJECTORY_NUMBER_OF_POINTS].set_value( - active_joint_traj_.points.size()); + max_trajectory_time_ = active_joint_traj_.points.back().time_from_start.sec + + (active_joint_traj_.points.back().time_from_start.nanosec / pow(10, 9)); - std::cout << "Size of positions: " << active_joint_traj_.points[0].positions.size() << std::endl; - - std::cout << "Size of velocities: " << active_joint_traj_.points[0].velocities.size() << std::endl; - - std::cout << "Size of accelerations: " << active_joint_traj_.points[0].accelerations.size() << std::endl; - - command_interfaces_[CommandInterfaces::PASSTHROUGH_POINT_WRITTEN].set_value(1.0); command_interfaces_[CommandInterfaces::PASSTHROUGH_TRAJECTORY_TRANSFER_STATE].set_value(1.0); + /* Start the executing thread of the action server, this will run until the trajectory is finished or cancelled. */ std::thread{ std::bind(&PassthroughTrajectoryController::execute, this, std::placeholders::_1), goal_handle } .detach(); return; } void PassthroughTrajectoryController::execute( - const std::shared_ptr> goal_handle) + const std::shared_ptr> goal_handle) { + /* While loop should execute 200 times pr second. Can be made slower if needed, but it will affect how fast the points + * of the trajectory are transferred to the hardware interface. */ rclcpp::Rate loop_rate(200); while (command_interfaces_[CommandInterfaces::PASSTHROUGH_TRAJECTORY_TRANSFER_STATE].get_value() != 0.0) { + /* Check if the trajectory has been cancelled from the hardware interface. E.g. the robot was stopped on the teach + * pendant. */ if (command_interfaces_[PASSTHROUGH_TRAJECTORY_CANCEL].get_value() == 1.0) { RCLCPP_INFO(get_node()->get_logger(), "Trajectory cancelled from hardware interface, aborting action."); - std::shared_ptr result = - std::make_shared(); + std::shared_ptr result = + std::make_shared(); goal_handle->abort(result); - command_interfaces_[CommandInterfaces::PASSTHROUGH_TRAJECTORY_TRANSFER_STATE].set_value(0.0); - trajectory_active_ = false; + end_goal(); return; } - + /* Check if the goal has been cancelled by the ROS user. */ if (goal_handle->is_canceling()) { - std::shared_ptr result = - std::make_shared(); + std::shared_ptr result = + std::make_shared(); goal_handle->canceled(result); + end_goal(); return; } - + // Write a new point to the command interface, if the previous point has been read by the hardware interface. if (command_interfaces_[CommandInterfaces::PASSTHROUGH_TRAJECTORY_TRANSFER_STATE].get_value() == 1.0) { - // Write a new point to the command interface, if the previous point has been written to the hardware interface. - if (command_interfaces_[CommandInterfaces::PASSTHROUGH_POINT_WRITTEN].get_value() == 1.0 && - current_point_ < active_joint_traj_.points.size()) { + if (current_point_ < active_joint_traj_.points.size()) { // Write the time_from_start parameter. command_interfaces_[CommandInterfaces::PASSTHROUGH_TRAJECTORY_TIME_FROM_START].set_value( active_joint_traj_.points[current_point_].time_from_start.sec + @@ -318,32 +342,64 @@ void PassthroughTrajectoryController::execute( } } // Tell hardware interface that this point is ready to be read. - command_interfaces_[CommandInterfaces::PASSTHROUGH_POINT_WRITTEN].set_value(0.0); + command_interfaces_[CommandInterfaces::PASSTHROUGH_TRAJECTORY_TRANSFER_STATE].set_value(2.0); current_point_++; - } - // Check if all points have been written to the hardware - if (command_interfaces_[CommandInterfaces::PASSTHROUGH_POINT_WRITTEN].get_value() == 1.0 && - current_point_ == active_joint_traj_.points.size()) { + + // Check if all points have been written to the hardware interface. + } else if (current_point_ == active_joint_traj_.points.size()) { RCLCPP_INFO(get_node()->get_logger(), "All points sent to the hardware interface, trajectory will now " "execute!"); - command_interfaces_[CommandInterfaces::PASSTHROUGH_TRAJECTORY_TRANSFER_STATE].set_value(2.0); + command_interfaces_[CommandInterfaces::PASSTHROUGH_TRAJECTORY_TRANSFER_STATE].set_value(3.0); } - } - - if (command_interfaces_[CommandInterfaces::PASSTHROUGH_TRAJECTORY_TRANSFER_STATE].get_value() == 4.0) { - std::shared_ptr result = - std::make_shared(); - goal_handle->succeed(result); - RCLCPP_INFO(get_node()->get_logger(), "Trajectory executed successfully!"); - std::cout << "It took this long: " << active_trajectory_elapsed_time_ << std::endl; - command_interfaces_[CommandInterfaces::PASSTHROUGH_TRAJECTORY_TRANSFER_STATE].set_value(0.0); - trajectory_active_ = false; + // When the trajectory is finished, report the goal as successful to the client. + } else if (command_interfaces_[CommandInterfaces::PASSTHROUGH_TRAJECTORY_TRANSFER_STATE].get_value() == 5.0) { + std::shared_ptr result = + std::make_shared(); + // Check if the actual position complies with the tolerances given. + if (!check_goal_tolerance()) { + result->error_code = control_msgs::action::FollowJointTrajectory::Result::GOAL_TOLERANCE_VIOLATED; + result->error_string = "Robot not within tolerances at end of trajectory."; + goal_handle->abort(result); + RCLCPP_ERROR(get_node()->get_logger(), "Trajectory failed, goal tolerances not met."); + // Check if the goal time tolerance was complied with. + } else if (active_trajectory_elapsed_time_ > (max_trajectory_time_ + goal_time_tolerance_.seconds())) { + result->error_code = control_msgs::action::FollowJointTrajectory::Result::GOAL_TOLERANCE_VIOLATED; + result->error_string = "Goal not reached within time tolerance"; + goal_handle->abort(result); + RCLCPP_ERROR(get_node()->get_logger(), "Trajectory failed, goal time tolerance not met."); + } else { + result->error_code = control_msgs::action::FollowJointTrajectory::Result::SUCCESSFUL; + goal_handle->succeed(result); + RCLCPP_INFO(get_node()->get_logger(), "Trajectory executed successfully!"); + } + end_goal(); return; } loop_rate.sleep(); } return; } + +bool PassthroughTrajectoryController::check_goal_tolerance() +{ + for (uint32_t i = 0; i < goal_tolerance_.size(); i++) { + if (std::abs(joint_position_state_interface_[i].get().get_value() - active_joint_traj_.points.back().positions[i]) > + goal_tolerance_[i].position) { + return false; + } + } + return true; +} + +void PassthroughTrajectoryController::end_goal() +{ + trajectory_active_ = false; + command_interfaces_[CommandInterfaces::PASSTHROUGH_TRAJECTORY_TRANSFER_STATE].set_value(0.0); + active_goal_ = NULL; + current_point_ = 0; + goal_tolerance_.clear(); + path_tolerance_.clear(); +} } // namespace ur_controllers #include "pluginlib/class_list_macros.hpp" diff --git a/ur_robot_driver/config/ur_controllers.yaml b/ur_robot_driver/config/ur_controllers.yaml index 3908d947e..40b3f5025 100644 --- a/ur_robot_driver/config/ur_controllers.yaml +++ b/ur_robot_driver/config/ur_controllers.yaml @@ -107,6 +107,31 @@ scaled_joint_trajectory_controller: $(var tf_prefix)wrist_3_joint: { trajectory: 0.2, goal: 0.1 } speed_scaling_interface_name: $(var tf_prefix)speed_scaling/speed_scaling_factor +passthrough_trajectory_controller: + ros__parameters: + tf_prefix: "$(var tf_prefix)" + joints: + - $(var tf_prefix)shoulder_pan_joint + - $(var tf_prefix)shoulder_lift_joint + - $(var tf_prefix)elbow_joint + - $(var tf_prefix)wrist_1_joint + - $(var tf_prefix)wrist_2_joint + - $(var tf_prefix)wrist_3_joint + state_interfaces: + - position + - velocity + - effort + constraints: + stopped_velocity_tolerance: 0.2 + goal_time: 0.0 + $(var tf_prefix)shoulder_pan_joint: { trajectory: 0.2, goal: 0.1 } + $(var tf_prefix)shoulder_lift_joint: { trajectory: 0.2, goal: 0.1 } + $(var tf_prefix)elbow_joint: { trajectory: 0.2, goal: 0.1 } + $(var tf_prefix)wrist_1_joint: { trajectory: 0.2, goal: 0.1 } + $(var tf_prefix)wrist_2_joint: { trajectory: 0.2, goal: 0.1 } + $(var tf_prefix)wrist_3_joint: { trajectory: 0.2, goal: 0.1 } + speed_scaling_interface_name: $(var tf_prefix)speed_scaling/speed_scaling_factor + forward_velocity_controller: ros__parameters: joints: diff --git a/ur_robot_driver/include/ur_robot_driver/hardware_interface.hpp b/ur_robot_driver/include/ur_robot_driver/hardware_interface.hpp index c506f985d..c259e157b 100644 --- a/ur_robot_driver/include/ur_robot_driver/hardware_interface.hpp +++ b/ur_robot_driver/include/ur_robot_driver/hardware_interface.hpp @@ -195,13 +195,14 @@ class URPositionHardwareInterface : public hardware_interface::SystemInterface bool async_thread_shutdown_; double passthrough_trajectory_transfer_state_; double passthrough_trajectory_cancel_; - double passthrough_point_written_; - double passthrough_trajectory_number_of_points_; double passthrough_trajectory_dimensions_; - std::array passthrough_trajectory_positions_; - std::array passthrough_trajectory_velocities_; - std::array passthrough_trajectory_accelerations_; + double passthrough_trajectory_controller_running_; + // TODO(URJala): The size of these arrays should be dependent on the number of joints on the physical robot. + urcl::vector6d_t passthrough_trajectory_positions_; + urcl::vector6d_t passthrough_trajectory_velocities_; + urcl::vector6d_t passthrough_trajectory_accelerations_; double passthrough_trajectory_time_from_start_; + double number_of_joints_; // payload stuff urcl::vector3d_t payload_center_of_gravity_; @@ -223,7 +224,6 @@ class URPositionHardwareInterface : public hardware_interface::SystemInterface bool robot_program_running_; bool non_blocking_read_; double robot_program_running_copy_; - bool passthrough_trajectory_executing_; std::vector> trajectory_joint_positions_; std::vector> trajectory_joint_velocities_; std::vector> trajectory_joint_accelerations_; @@ -237,7 +237,6 @@ class URPositionHardwareInterface : public hardware_interface::SystemInterface std::vector start_modes_; bool position_controller_running_; bool velocity_controller_running_; - bool passthrough_trajectory_controller_running_; std::unique_ptr ur_driver_; std::shared_ptr async_thread_; diff --git a/ur_robot_driver/src/hardware_interface.cpp b/ur_robot_driver/src/hardware_interface.cpp index 592d91ce1..94de1cd90 100644 --- a/ur_robot_driver/src/hardware_interface.cpp +++ b/ur_robot_driver/src/hardware_interface.cpp @@ -94,11 +94,12 @@ URPositionHardwareInterface::on_init(const hardware_interface::HardwareInfo& sys initialized_ = false; async_thread_shutdown_ = false; system_interface_initialized_ = 0.0; - passthrough_trajectory_executing_ = false; passthrough_trajectory_transfer_state_ = 0.0; + passthrough_trajectory_cancel_ = 0.0; trajectory_joint_positions_.clear(); trajectory_joint_velocities_.clear(); trajectory_joint_accelerations_.clear(); + number_of_joints_ = info_.joints.size(); for (const hardware_interface::ComponentInfo& joint : info_.joints) { if (joint.command_interfaces.size() != 2) { @@ -236,6 +237,12 @@ std::vector URPositionHardwareInterface::exp state_interfaces.emplace_back( hardware_interface::StateInterface(tf_prefix + "gpio", "program_running", &robot_program_running_copy_)); + state_interfaces.emplace_back( + hardware_interface::StateInterface(tf_prefix + "passthrough_controller", "number_of_joints", &number_of_joints_)); + + state_interfaces.emplace_back(hardware_interface::StateInterface(tf_prefix + "passthrough_controller", "running", + &passthrough_trajectory_controller_running_)); + return state_interfaces; } @@ -307,13 +314,6 @@ std::vector URPositionHardwareInterface::e "passthrough_trajectory_transfer_state", &passthrough_trajectory_transfer_state_)); - command_interfaces.emplace_back(hardware_interface::CommandInterface( - tf_prefix + "passthrough_controller", "passthrough_point_written", &passthrough_point_written_)); - - command_interfaces.emplace_back(hardware_interface::CommandInterface(tf_prefix + "passthrough_controller", - "passthrough_trajectory_number_of_points", - &passthrough_trajectory_number_of_points_)); - command_interfaces.emplace_back(hardware_interface::CommandInterface( tf_prefix + "passthrough_controller", "passthrough_trajectory_cancel", &passthrough_trajectory_cancel_)); @@ -682,7 +682,7 @@ hardware_interface::return_type URPositionHardwareInterface::write(const rclcpp: } else if (velocity_controller_running_) { ur_driver_->writeJointCommand(urcl_velocity_commands_, urcl::comm::ControlMode::MODE_SPEEDJ); - } else if (passthrough_trajectory_controller_running_ && passthrough_trajectory_executing_ != 1.0) { + } else if (passthrough_trajectory_controller_running_) { ur_driver_->writeTrajectoryControlMessage(urcl::control::TrajectoryControlMessage::TRAJECTORY_NOOP); } else { ur_driver_->writeKeepalive(); @@ -851,10 +851,9 @@ hardware_interface::return_type URPositionHardwareInterface::prepare_command_mod const std::vector& start_interfaces, const std::vector& stop_interfaces) { hardware_interface::return_type ret_val = hardware_interface::return_type::OK; - start_modes_.clear(); stop_modes_.clear(); - + const std::string tf_prefix = info_.hardware_parameters.at("tf_prefix"); // Starting interfaces // add start interface per joint in tmp var for later check for (const auto& key : start_interfaces) { @@ -865,7 +864,7 @@ hardware_interface::return_type URPositionHardwareInterface::prepare_command_mod if (key == info_.joints[i].name + "/" + hardware_interface::HW_IF_VELOCITY) { start_modes_.push_back(hardware_interface::HW_IF_VELOCITY); } - if (key == PASSTHROUGH_TRAJECTORY_CONTROLLER + std::to_string(i)) { + if (key == tf_prefix + PASSTHROUGH_TRAJECTORY_CONTROLLER + std::to_string(i)) { start_modes_.push_back(PASSTHROUGH_TRAJECTORY_CONTROLLER); } } @@ -920,33 +919,31 @@ hardware_interface::return_type URPositionHardwareInterface::perform_command_mod urcl_velocity_commands_ = { { 0.0, 0.0, 0.0, 0.0, 0.0, 0.0 } }; } else if (stop_modes_.size() != 0 && std::find(stop_modes_.begin(), stop_modes_.end(), StoppingInterface::STOP_PASSTHROUGH) != stop_modes_.end()) { - passthrough_trajectory_controller_running_ = false; - passthrough_trajectory_cancel_ = 1; + passthrough_trajectory_controller_running_ = 0.0; + passthrough_trajectory_cancel_ = 1.0; trajectory_joint_positions_.clear(); trajectory_joint_accelerations_.clear(); trajectory_joint_velocities_.clear(); - std::cout << "---------------------Stopped passthrough controller---------------" << std::endl; } if (start_modes_.size() != 0 && std::find(start_modes_.begin(), start_modes_.end(), hardware_interface::HW_IF_POSITION) != start_modes_.end()) { velocity_controller_running_ = false; - passthrough_trajectory_controller_running_ = false; + passthrough_trajectory_controller_running_ = 0.0; urcl_position_commands_ = urcl_position_commands_old_ = urcl_joint_positions_; position_controller_running_ = true; } else if (start_modes_.size() != 0 && std::find(start_modes_.begin(), start_modes_.end(), hardware_interface::HW_IF_VELOCITY) != start_modes_.end()) { position_controller_running_ = false; - passthrough_trajectory_controller_running_ = false; + passthrough_trajectory_controller_running_ = 0.0; urcl_velocity_commands_ = { { 0.0, 0.0, 0.0, 0.0, 0.0, 0.0 } }; velocity_controller_running_ = true; } else if (start_modes_.size() != 0 && std::find(start_modes_.begin(), start_modes_.end(), PASSTHROUGH_TRAJECTORY_CONTROLLER) != start_modes_.end()) { velocity_controller_running_ = false; position_controller_running_ = false; - passthrough_trajectory_controller_running_ = true; - std::cout << "---------------------Started passthrough controller---------------" << std::endl; + passthrough_trajectory_controller_running_ = 1.0; } start_modes_.clear(); @@ -958,27 +955,32 @@ hardware_interface::return_type URPositionHardwareInterface::perform_command_mod void URPositionHardwareInterface::check_passthrough_trajectory_controller() { static double last_time = 0.0; - if (passthrough_trajectory_transfer_state_ == 1.0 && passthrough_point_written_ == 0.0) { + /* See passthrough_trajectory_controller.hpp for an explanation of the passthrough_trajectory_transfer_state_ values. + */ + if (passthrough_trajectory_transfer_state_ == 2.0) { trajectory_joint_positions_.push_back(passthrough_trajectory_positions_); trajectory_times_.push_back(passthrough_trajectory_time_from_start_ - last_time); last_time = passthrough_trajectory_time_from_start_; - passthrough_point_written_ = 1.0; + if (passthrough_trajectory_dimensions_ == 2.0 || passthrough_trajectory_dimensions_ == 4.0) { trajectory_joint_velocities_.push_back(passthrough_trajectory_velocities_); } if (passthrough_trajectory_dimensions_ == 3.0 || passthrough_trajectory_dimensions_ == 4.0) { trajectory_joint_accelerations_.push_back(passthrough_trajectory_accelerations_); } - } else if (passthrough_trajectory_transfer_state_ == 2.0) { - std::cout << " Passing through " + std::to_string(trajectory_joint_positions_.size()) + " Points to the robot." - << std::endl; - + passthrough_trajectory_transfer_state_ = 1.0; + /* When all points have been read, write them to the robot driver.*/ + } else if (passthrough_trajectory_transfer_state_ == 3.0) { + /* Tell robot driver how many points are in the trajectory. */ ur_driver_->writeTrajectoryControlMessage(urcl::control::TrajectoryControlMessage::TRAJECTORY_START, trajectory_joint_positions_.size()); + /* Write the appropriate type of point according to the dimensions of the trajectory. See + * passthrough_trajectory_controller.hpp - check_dimensions() for an explanation of the values. */ if (passthrough_trajectory_dimensions_ == 1.0) { for (size_t i = 0; i < trajectory_joint_positions_.size(); i++) { - ur_driver_->writeTrajectoryPoint(trajectory_joint_positions_[i], false, trajectory_times_[i]); + ur_driver_->writeTrajectorySplinePoint(trajectory_joint_positions_[i], urcl::vector6d_t{ 0, 0, 0, 0, 0, 0 }, + trajectory_times_[i]); } } else if (passthrough_trajectory_dimensions_ == 2.0) { for (size_t i = 0; i < trajectory_joint_positions_.size(); i++) { @@ -999,21 +1001,17 @@ void URPositionHardwareInterface::check_passthrough_trajectory_controller() trajectory_joint_positions_.clear(); trajectory_joint_accelerations_.clear(); trajectory_joint_velocities_.clear(); - passthrough_trajectory_transfer_state_ = 3.0; + passthrough_trajectory_transfer_state_ = 4.0; } } void URPositionHardwareInterface::trajectory_done_callback(urcl::control::TrajectoryResult result) { - std::cout << "-------------------Triggered trajectory callback!--------------------------" << std::endl; if (result == urcl::control::TrajectoryResult::TRAJECTORY_RESULT_SUCCESS) { - passthrough_trajectory_transfer_state_ = 4.0; - std::cout << "-------------------Trajectory was successful!--------------------------" << std::endl; + passthrough_trajectory_transfer_state_ = 5.0; } else { passthrough_trajectory_cancel_ = 1.0; } - std::cout << "Result is: " << int(result) << std::endl; - passthrough_trajectory_executing_ = false; return; } diff --git a/ur_robot_driver/test/__pycache__/robot_driver.cpython-312.pyc b/ur_robot_driver/test/__pycache__/robot_driver.cpython-312.pyc new file mode 100644 index 000000000..3dbbf549a Binary files /dev/null and b/ur_robot_driver/test/__pycache__/robot_driver.cpython-312.pyc differ diff --git a/ur_robot_driver/test/__pycache__/test_common.cpython-312.pyc b/ur_robot_driver/test/__pycache__/test_common.cpython-312.pyc new file mode 100644 index 000000000..ead048f62 Binary files /dev/null and b/ur_robot_driver/test/__pycache__/test_common.cpython-312.pyc differ diff --git a/ur_robot_driver/test/robot_driver.py b/ur_robot_driver/test/robot_driver.py index 0805acbff..8182b0c03 100755 --- a/ur_robot_driver/test/robot_driver.py +++ b/ur_robot_driver/test/robot_driver.py @@ -37,10 +37,11 @@ import rclpy from builtin_interfaces.msg import Duration from control_msgs.action import FollowJointTrajectory +from control_msgs.msg import JointTolerance from controller_manager_msgs.srv import SwitchController from rclpy.node import Node from sensor_msgs.msg import JointState -from trajectory_msgs.msg import JointTrajectory, JointTrajectoryPoint +from trajectory_msgs.msg import JointTrajectory as JointTrajectory, JointTrajectoryPoint from ur_msgs.msg import IOStates sys.path.append(os.path.dirname(__file__)) @@ -98,6 +99,11 @@ def init_robot(self): "/scaled_joint_trajectory_controller/follow_joint_trajectory", FollowJointTrajectory, ) + self._passthrough_forward_joint_trajectory = ActionInterface( + self.node, + "/passthrough_trajectory_controller/forward_joint_trajectory", + FollowJointTrajectory, + ) def setUp(self): self._dashboard_interface.start_robot() @@ -121,6 +127,14 @@ def test_start_passthrough_controller(self): self._controller_manager_interface.switch_controller( strictness=SwitchController.Request.BEST_EFFORT, activate_controllers=["passthrough_trajectory_controller"], + deactivate_controllers=["scaled_joint_trajectory_controller"], + ).ok + ) + self.assertTrue( + self._controller_manager_interface.switch_controller( + strictness=SwitchController.Request.BEST_EFFORT, + deactivate_controllers=["passthrough_trajectory_controller"], + activate_controllers=["scaled_joint_trajectory_controller"], ).ok ) @@ -333,3 +347,82 @@ def js_cb(msg): # result = self.get_result("/scaled_joint_trajectory_controller/follow_joint_trajectory", goal_response, TIMEOUT_EXECUTE_TRAJECTORY) # self.assertEqual(result.error_code, FollowJointTrajectory.Result.GOAL_TOLERANCE_VIOLATED) # self.node.get_logger().info("Received result GOAL_TOLERANCE_VIOLATED") + + def test_passthrough_trajectory(self): + self.assertTrue( + self._controller_manager_interface.switch_controller( + strictness=SwitchController.Request.BEST_EFFORT, + activate_controllers=["passthrough_trajectory_controller"], + deactivate_controllers=["scaled_joint_trajectory_controller"], + ).ok + ) + waypts = [ + [-1.58, -1.692, -1.4311, -0.0174, 1.5882, 0.0349], + [-3, -1.692, -1.4311, -0.0174, 1.5882, 0.0349], + [-1.58, -1.692, -1.4311, -0.0174, 1.5882, 0.0349], + ] + time_vec = [ + Duration(sec=4, nanosec=0), + Duration(sec=8, nanosec=0), + Duration(sec=12, nanosec=0), + ] + goal_tolerance = [JointTolerance(position=0.01) for i in range(6)] + goal_time_tolerance = Duration(sec=1, nanosec=0) + test_trajectory = zip(time_vec, waypts) + trajectory = JointTrajectory( + points=[ + JointTrajectoryPoint(positions=pos, time_from_start=times) + for (times, pos) in test_trajectory + ], + ) + goal_handle = self._passthrough_forward_joint_trajectory.send_goal( + trajectory=trajectory, + goal_time_tolerance=goal_time_tolerance, + goal_tolerance=goal_tolerance, + ) + self.assertTrue(goal_handle.accepted) + if goal_handle.accepted: + result = self._passthrough_forward_joint_trajectory.get_result( + goal_handle, TIMEOUT_EXECUTE_TRAJECTORY + ) + self.assertEqual(result.error_code, FollowJointTrajectory.Result.SUCCESSFUL) + # Test impossible goal tolerance, should fail. + goal_tolerance = [JointTolerance(position=0.000000000000000001) for i in range(6)] + goal_handle = self._passthrough_forward_joint_trajectory.send_goal( + trajectory=trajectory, + goal_time_tolerance=goal_time_tolerance, + goal_tolerance=goal_tolerance, + ) + self.assertTrue(goal_handle.accepted) + if goal_handle.accepted: + result = self._passthrough_forward_joint_trajectory.get_result( + goal_handle, TIMEOUT_EXECUTE_TRAJECTORY + ) + self.assertEqual( + result.error_code, FollowJointTrajectory.Result.GOAL_TOLERANCE_VIOLATED + ) + + # Test impossible goal time + goal_tolerance = [JointTolerance(position=0.01) for i in range(6)] + goal_time_tolerance.sec = 0 + goal_time_tolerance.nanosec = 1000 + goal_handle = self._passthrough_forward_joint_trajectory.send_goal( + trajectory=trajectory, + goal_time_tolerance=goal_time_tolerance, + goal_tolerance=goal_tolerance, + ) + self.assertTrue(goal_handle.accepted) + if goal_handle.accepted: + result = self._passthrough_forward_joint_trajectory.get_result( + goal_handle, TIMEOUT_EXECUTE_TRAJECTORY + ) + self.assertEqual( + result.error_code, FollowJointTrajectory.Result.GOAL_TOLERANCE_VIOLATED + ) + self.assertTrue( + self._controller_manager_interface.switch_controller( + strictness=SwitchController.Request.BEST_EFFORT, + deactivate_controllers=["passthrough_trajectory_controller"], + activate_controllers=["scaled_joint_trajectory_controller"], + ).ok + ) diff --git a/ur_robot_driver/test/test_common.py b/ur_robot_driver/test/test_common.py index 8dec224a3..c4af8eb55 100644 --- a/ur_robot_driver/test/test_common.py +++ b/ur_robot_driver/test/test_common.py @@ -29,16 +29,20 @@ import time import rclpy -from controller_manager_msgs.srv import ListControllers, SwitchController +from controller_manager_msgs.srv import ( + ListControllers, + SwitchController, + LoadController, + UnloadController, +) from launch import LaunchDescription from launch.actions import ( DeclareLaunchArgument, ExecuteProcess, IncludeLaunchDescription, - # RegisterEventHandler, + RegisterEventHandler, ) - -# from launch.event_handlers import OnProcessExit +from launch.event_handlers import OnProcessExit from launch.launch_description_sources import PythonLaunchDescriptionSource from launch.substitutions import LaunchConfiguration, PathJoinSubstitution from launch_ros.substitutions import FindPackagePrefix, FindPackageShare @@ -224,7 +228,11 @@ def _check_call(self, result): class ControllerManagerInterface( _ServiceInterface, namespace="/controller_manager", - initial_services={"switch_controller": SwitchController}, + initial_services={ + "switch_controller": SwitchController, + "load_controller": LoadController, + "unload_controller": UnloadController, + }, services={"list_controllers": ListControllers}, ): def wait_for_controller(self, controller_name, target_state="active"): @@ -316,7 +324,7 @@ def generate_driver_test_description( ) ), launch_arguments={ - "robot_ip": "172.17.0.3", + "robot_ip": "192.168.56.101", "ur_type": ur_type, "launch_rviz": "false", "controller_spawner_timeout": str(controller_spawner_timeout), @@ -327,17 +335,20 @@ def generate_driver_test_description( "tf_prefix": tf_prefix, }.items(), ) - # wait_dashboard_server = ExecuteProcess( - # cmd=[ - # PathJoinSubstitution( - # [FindPackagePrefix("ur_robot_driver"), "bin", "wait_dashboard_server.sh"] - # ) - # ], - # name="wait_dashboard_server", - # output="screen", - # ) - # driver_starter = RegisterEventHandler( - # OnProcessExit(target_action=wait_dashboard_server, on_exit=robot_driver) - # ) - - return LaunchDescription(_declare_launch_arguments() + [ReadyToTest(), robot_driver]) + wait_dashboard_server = ExecuteProcess( + cmd=[ + PathJoinSubstitution( + [FindPackagePrefix("ur_robot_driver"), "bin", "wait_dashboard_server.sh"] + ) + ], + name="wait_dashboard_server", + output="screen", + ) + driver_starter = RegisterEventHandler( + OnProcessExit(target_action=wait_dashboard_server, on_exit=robot_driver) + ) + + return LaunchDescription( + _declare_launch_arguments() + + [ReadyToTest(), wait_dashboard_server, _ursim_action(), driver_starter] + )