From 26fbf5f4dd22b3cee6c3a3575a31fea94ae14316 Mon Sep 17 00:00:00 2001 From: mhubii Date: Tue, 11 Jun 2024 11:08:11 +0100 Subject: [PATCH 01/15] preparation for new controllers --- .../lbr_demos_advanced_cpp/CMakeLists.txt | 30 +----- .../src/admittance_control_node.cpp | 93 ------------------- .../src/admittance_controller.hpp | 90 ------------------ .../src/lbr_base_position_command_node.hpp | 2 +- lbr_fri_ros2/include/lbr_fri_ros2/app.hpp | 2 +- .../include/lbr_fri_ros2/async_client.hpp | 2 +- .../include/lbr_fri_ros2/command_guard.hpp | 2 +- lbr_fri_ros2/include/lbr_fri_ros2/filters.hpp | 2 +- .../include/lbr_fri_ros2/formatting.hpp | 2 +- .../include/lbr_fri_ros2/ft_estimator.hpp | 2 +- .../interfaces/position_command.hpp | 2 +- .../include/lbr_fri_ros2/interfaces/state.hpp | 2 +- .../interfaces/torque_command.hpp | 2 +- .../interfaces/wrench_command.hpp | 2 +- lbr_fri_ros2/include/lbr_fri_ros2/pinv.hpp | 2 +- lbr_fri_ros2/src/app.cpp | 2 +- lbr_fri_ros2/src/async_client.cpp | 2 +- lbr_fri_ros2/src/command_guard.cpp | 2 +- lbr_fri_ros2/src/filters.cpp | 2 +- lbr_fri_ros2/src/ft_estimator.cpp | 2 +- .../src/interfaces/position_command.cpp | 2 +- .../src/interfaces/torque_command.cpp | 2 +- .../src/interfaces/wrench_command.cpp | 2 +- lbr_ros2_control/CMakeLists.txt | 11 +-- lbr_ros2_control/config/lbr_controllers.yaml | 6 +- .../controllers/admittance_controller.hpp | 63 +++++++++++++ .../lbr_joint_position_command_controller.hpp | 2 +- .../lbr_state_broadcaster.hpp | 2 +- .../lbr_torque_command_controller.hpp | 2 +- .../lbr_wrench_command_controller.hpp | 2 +- .../lbr_ros2_control/system_interface.hpp | 2 +- .../system_interface_type_values.hpp | 2 +- .../plugin_description_files/broadcasters.xml | 8 -- ...ommand_controllers.xml => controllers.xml} | 13 +++ .../src/controllers/admittance_controller.cpp | 77 +++++++++++++++ .../lbr_joint_position_command_controller.cpp | 4 +- .../lbr_state_broadcaster.cpp | 4 +- .../lbr_torque_command_controller.cpp | 4 +- .../lbr_wrench_command_controller.cpp | 4 +- lbr_ros2_control/src/system_interface.cpp | 2 +- 40 files changed, 199 insertions(+), 262 deletions(-) delete mode 100644 lbr_demos/lbr_demos_advanced_cpp/src/admittance_control_node.cpp delete mode 100644 lbr_demos/lbr_demos_advanced_cpp/src/admittance_controller.hpp create mode 100644 lbr_ros2_control/include/lbr_ros2_control/controllers/admittance_controller.hpp rename lbr_ros2_control/include/lbr_ros2_control/{ => controllers}/lbr_joint_position_command_controller.hpp (98%) rename lbr_ros2_control/include/lbr_ros2_control/{ => controllers}/lbr_state_broadcaster.hpp (98%) rename lbr_ros2_control/include/lbr_ros2_control/{ => controllers}/lbr_torque_command_controller.hpp (98%) rename lbr_ros2_control/include/lbr_ros2_control/{ => controllers}/lbr_wrench_command_controller.hpp (98%) delete mode 100644 lbr_ros2_control/plugin_description_files/broadcasters.xml rename lbr_ros2_control/plugin_description_files/{forward_command_controllers.xml => controllers.xml} (67%) create mode 100644 lbr_ros2_control/src/controllers/admittance_controller.cpp rename lbr_ros2_control/src/{ => controllers}/lbr_joint_position_command_controller.cpp (96%) rename lbr_ros2_control/src/{ => controllers}/lbr_state_broadcaster.cpp (98%) rename lbr_ros2_control/src/{ => controllers}/lbr_torque_command_controller.cpp (97%) rename lbr_ros2_control/src/{ => controllers}/lbr_wrench_command_controller.cpp (97%) diff --git a/lbr_demos/lbr_demos_advanced_cpp/CMakeLists.txt b/lbr_demos/lbr_demos_advanced_cpp/CMakeLists.txt index 14040864..b4a883db 100644 --- a/lbr_demos/lbr_demos_advanced_cpp/CMakeLists.txt +++ b/lbr_demos/lbr_demos_advanced_cpp/CMakeLists.txt @@ -20,34 +20,6 @@ find_package(orocos_kdl_vendor REQUIRED) find_package(rclcpp REQUIRED) find_package(rclcpp_components REQUIRED) -# admittance control node -add_library(admittance_control_component - SHARED - src/admittance_control_node.cpp -) - -target_include_directories(admittance_control_component - PRIVATE src -) - -ament_target_dependencies(admittance_control_component - kdl_parser - lbr_fri_idl - lbr_fri_ros2 - orocos_kdl_vendor - rclcpp - rclcpp_components -) - -target_link_libraries(admittance_control_component - FRIClient::FRIClient -) - -rclcpp_components_register_node(admittance_control_component - PLUGIN lbr_demos::AdmittanceControlNode - EXECUTABLE admittance_control -) - # pose planning node add_library(pose_planning_component SHARED @@ -99,7 +71,7 @@ rclcpp_components_register_node(pose_control_component # install install( - TARGETS admittance_control_component pose_planning_component pose_control_component + TARGETS pose_planning_component pose_control_component LIBRARY DESTINATION lib RUNTIME DESTINATION lib/${PROJECT_NAME} ) diff --git a/lbr_demos/lbr_demos_advanced_cpp/src/admittance_control_node.cpp b/lbr_demos/lbr_demos_advanced_cpp/src/admittance_control_node.cpp deleted file mode 100644 index e3e06741..00000000 --- a/lbr_demos/lbr_demos_advanced_cpp/src/admittance_control_node.cpp +++ /dev/null @@ -1,93 +0,0 @@ -#include - -#include "rclcpp/rclcpp.hpp" - -#include "lbr_fri_idl/msg/lbr_state.hpp" - -#include "admittance_controller.hpp" -#include "lbr_base_position_command_node.hpp" - -namespace lbr_demos { -class AdmittanceControlNode : public LBRBasePositionCommandNode { -public: - AdmittanceControlNode(const rclcpp::NodeOptions &options) - : LBRBasePositionCommandNode("admittance_control", options) { - this->declare_parameter("base_link", "link_0"); - this->declare_parameter("end_effector_link", "link_ee"); - this->declare_parameter>("f_ext_th", {2., 2., 2., 0.5, 0.5, 0.5}); - this->declare_parameter>("dq_gains", {2., 2., 2., 2., 2., 2., 2.}); - this->declare_parameter>("dx_gains", {0.1, 0.1, 0.1, 0.1, 0.1, 0.1}); - this->declare_parameter("exp_smooth", 0.95); - - exp_smooth_ = this->get_parameter("exp_smooth").as_double(); - if (exp_smooth_ < 0. || exp_smooth_ > 1.) { - throw std::runtime_error("Invalid exponential smoothing factor."); - } - - admittance_controller_ = std::make_unique( - this->robot_description_, this->get_parameter("base_link").as_string(), - this->get_parameter("end_effector_link").as_string(), - this->get_parameter("f_ext_th").as_double_array(), - this->get_parameter("dq_gains").as_double_array(), - this->get_parameter("dx_gains").as_double_array()); - - // log parameters to terminal - this->log_paramters_(); - } - -protected: - void log_paramters_() { - RCLCPP_INFO(this->get_logger(), "*** Parameters:"); - RCLCPP_INFO(this->get_logger(), "* base_link: %s", - this->get_parameter("base_link").as_string().c_str()); - RCLCPP_INFO(this->get_logger(), "* end_effector_link: %s", - this->get_parameter("end_effector_link").as_string().c_str()); - auto f_ext_th = this->get_parameter("f_ext_th").as_double_array(); - RCLCPP_INFO(this->get_logger(), "* f_ext_th: %.1f, %.1f, %.1f, %.1f, %.1f, %.1f", f_ext_th[0], - f_ext_th[1], f_ext_th[2], f_ext_th[3], f_ext_th[4], f_ext_th[5]); - auto dq_gains = this->get_parameter("dq_gains").as_double_array(); - RCLCPP_INFO(this->get_logger(), "* dq_gains: %.1f, %.1f, %.1f, %.1f, %.1f, %.1f, %.1f", - dq_gains[0], dq_gains[1], dq_gains[2], dq_gains[3], dq_gains[4], dq_gains[5], - dq_gains[6]); - auto dx_gains = this->get_parameter("dx_gains").as_double_array(); - RCLCPP_INFO(this->get_logger(), "* dx_gains: %.1f, %.1f, %.1f, %.1f, %.1f, %.1f", dx_gains[0], - dx_gains[1], dx_gains[2], dx_gains[3], dx_gains[4], dx_gains[5]); - } - - void on_lbr_state_(const lbr_fri_idl::msg::LBRState::SharedPtr lbr_state) override { - if (!lbr_state) { - return; - } - - smooth_lbr_state_(lbr_state); - - auto lbr_command = admittance_controller_->update(lbr_state_, dt_); - lbr_joint_position_command_pub_->publish(lbr_command); - }; - - void smooth_lbr_state_(const lbr_fri_idl::msg::LBRState::SharedPtr lbr_state) { - if (!init_) { - lbr_state_ = *lbr_state; - init_ = true; - return; - } - - for (int i = 0; i < 7; i++) { - lbr_state_.measured_joint_position[i] = - lbr_state->measured_joint_position[i] * (1 - exp_smooth_) + - lbr_state_.measured_joint_position[i] * exp_smooth_; - lbr_state_.external_torque[i] = lbr_state->external_torque[i] * (1 - exp_smooth_) + - lbr_state_.external_torque[i] * exp_smooth_; - } - } - - double exp_smooth_; - bool init_{false}; - lbr_fri_idl::msg::LBRState lbr_state_; - - std::unique_ptr admittance_controller_; -}; -} // end of namespace lbr_demos - -#include "rclcpp_components/register_node_macro.hpp" -RCLCPP_COMPONENTS_REGISTER_NODE(lbr_demos::AdmittanceControlNode) diff --git a/lbr_demos/lbr_demos_advanced_cpp/src/admittance_controller.hpp b/lbr_demos/lbr_demos_advanced_cpp/src/admittance_controller.hpp deleted file mode 100644 index 3a12894d..00000000 --- a/lbr_demos/lbr_demos_advanced_cpp/src/admittance_controller.hpp +++ /dev/null @@ -1,90 +0,0 @@ -#ifndef LBR_DEMOS_ADVANCED_CPP__ADMITTANCE_CONTROLLER_HPP_ -#define LBR_DEMOS_ADVANCED_CPP__ADMITTANCE_CONTROLLER_HPP_ - -#include -#include -#include - -#include "kdl/chain.hpp" -#include "kdl/chainjnttojacsolver.hpp" -#include "kdl/tree.hpp" -#include "kdl_parser/kdl_parser.hpp" - -#include "friLBRState.h" - -#include "lbr_fri_idl/msg/lbr_joint_position_command.hpp" -#include "lbr_fri_idl/msg/lbr_state.hpp" -#include "lbr_fri_ros2/pinv.hpp" - -namespace lbr_demos { -class AdmittanceController { - using joint_vector_t = Eigen::Vector; - using cartesian_vector_t = Eigen::Vector; - -public: - AdmittanceController(const std::string &robot_description, - const std::string &base_link = "link_0", - const std::string &end_effector_link = "link_ee", - const std::vector &f_ext_th = {2., 2., 2., 0.5, 0.5, 0.5}, - const std::vector &dq_gains = {2., 2., 2., 2., 2., 2., 2.}, - const std::vector &dx_gains = {0.1, 0.1, 0.1, 0.1, 0.1, 0.1}) - : dq_gains_(dq_gains.data()), dx_gains_(dx_gains.data()), f_ext_th_(f_ext_th.data()) { - if (!kdl_parser::treeFromString(robot_description, tree_)) { - throw std::runtime_error("Failed to construct kdl tree from robot description."); - } - if (!tree_.getChain(base_link, end_effector_link, chain_)) { - throw std::runtime_error("Failed to construct kdl chain from robot description."); - } - - jacobian_solver_ = std::make_unique(chain_); - jacobian_.resize(chain_.getNrOfJoints()); - q_.resize(chain_.getNrOfJoints()); - }; - - const lbr_fri_idl::msg::LBRJointPositionCommand & - update(const lbr_fri_idl::msg::LBRState &lbr_state, const double &dt) { - std::memcpy(q_.data.data(), lbr_state.measured_joint_position.data(), - sizeof(double) * KUKA::FRI::LBRState::NUMBER_OF_JOINTS); - std::memcpy(tau_ext_.data(), lbr_state.external_torque.data(), - sizeof(double) * KUKA::FRI::LBRState::NUMBER_OF_JOINTS); - - jacobian_solver_->JntToJac(q_, jacobian_); - - jacobian_inv_ = lbr_fri_ros2::pinv(jacobian_.data); - f_ext_ = jacobian_inv_.transpose() * tau_ext_; - - for (int i = 0; i < 6; i++) { - if (std::abs(f_ext_[i]) < f_ext_th_[i]) { - f_ext_[i] = 0.; - } else { - f_ext_[i] = std::copysign(dx_gains_[i], f_ext_[i]) * (std::abs(f_ext_[i]) - f_ext_th_[i]); - } - } - - dq_ = dq_gains_.asDiagonal() * jacobian_inv_ * f_ext_; - - for (int i = 0; i < 7; i++) { - lbr_joint_position_command_.joint_position[i] = - lbr_state.measured_joint_position[i] + dq_[i] * dt; - } - - return lbr_joint_position_command_; - }; - -protected: - lbr_fri_idl::msg::LBRJointPositionCommand lbr_joint_position_command_; - KDL::Tree tree_; - KDL::Chain chain_; - std::unique_ptr jacobian_solver_; - KDL::Jacobian jacobian_; - Eigen::Matrix jacobian_inv_; - KDL::JntArray q_; - joint_vector_t dq_; - joint_vector_t tau_ext_; - joint_vector_t dq_gains_; - cartesian_vector_t dx_gains_; - cartesian_vector_t f_ext_; - cartesian_vector_t f_ext_th_; -}; -} // end of namespace lbr_demos -#endif // LBR_DEMOS_ADVANCED_CPP__ADMITTANCE_CONTROLLER_HPP_ diff --git a/lbr_demos/lbr_demos_advanced_cpp/src/lbr_base_position_command_node.hpp b/lbr_demos/lbr_demos_advanced_cpp/src/lbr_base_position_command_node.hpp index 8fec33db..e318586e 100644 --- a/lbr_demos/lbr_demos_advanced_cpp/src/lbr_base_position_command_node.hpp +++ b/lbr_demos/lbr_demos_advanced_cpp/src/lbr_base_position_command_node.hpp @@ -64,5 +64,5 @@ class LBRBasePositionCommandNode : public rclcpp::Node { return future.get()[0]; } }; -} // end of namespace lbr_demos +} // namespace lbr_demos #endif // LBR_DEMOS_ADVANCED_CPP__LBR_BASE_POSITION_COMMAND_NODE_HPP_ diff --git a/lbr_fri_ros2/include/lbr_fri_ros2/app.hpp b/lbr_fri_ros2/include/lbr_fri_ros2/app.hpp index 3aea94f2..f66a8af0 100644 --- a/lbr_fri_ros2/include/lbr_fri_ros2/app.hpp +++ b/lbr_fri_ros2/include/lbr_fri_ros2/app.hpp @@ -39,5 +39,5 @@ class App { std::unique_ptr connection_ptr_; std::unique_ptr app_ptr_; }; -} // end of namespace lbr_fri_ros2 +} // namespace lbr_fri_ros2 #endif // LBR_FRI_ROS2__APP_HPP_ diff --git a/lbr_fri_ros2/include/lbr_fri_ros2/async_client.hpp b/lbr_fri_ros2/include/lbr_fri_ros2/async_client.hpp index 2696ca44..5e8a8c47 100644 --- a/lbr_fri_ros2/include/lbr_fri_ros2/async_client.hpp +++ b/lbr_fri_ros2/include/lbr_fri_ros2/async_client.hpp @@ -50,5 +50,5 @@ class AsyncClient : public KUKA::FRI::LBRClient { bool open_loop_; }; -} // end of namespace lbr_fri_ros2 +} // namespace lbr_fri_ros2 #endif // LBR_FRI_ROS2__ASYNC_CLIENT_HPP_ diff --git a/lbr_fri_ros2/include/lbr_fri_ros2/command_guard.hpp b/lbr_fri_ros2/include/lbr_fri_ros2/command_guard.hpp index 41c10c61..c332e270 100644 --- a/lbr_fri_ros2/include/lbr_fri_ros2/command_guard.hpp +++ b/lbr_fri_ros2/include/lbr_fri_ros2/command_guard.hpp @@ -76,5 +76,5 @@ class SafeStopCommandGuard : public CommandGuard { std::unique_ptr command_guard_factory(const CommandGuardParameters &command_guard_parameters, const std::string &variant = "default"); -} // end of namespace lbr_fri_ros2 +} // namespace lbr_fri_ros2 #endif // LBR_FRI_ROS2__COMMAND_GUARD_HPP_ diff --git a/lbr_fri_ros2/include/lbr_fri_ros2/filters.hpp b/lbr_fri_ros2/include/lbr_fri_ros2/filters.hpp index bf72faa3..f14d5915 100644 --- a/lbr_fri_ros2/include/lbr_fri_ros2/filters.hpp +++ b/lbr_fri_ros2/include/lbr_fri_ros2/filters.hpp @@ -139,5 +139,5 @@ class JointPIDArray { PIDParameters pid_parameters_; /**< PID parameters for all joints.*/ pid_array_t pid_controllers_; /**< PID controllers for each joint.*/ }; -} // end of namespace lbr_fri_ros2 +} // namespace lbr_fri_ros2 #endif // LBR_FRI_ROS2__FILTERS_HPP_ diff --git a/lbr_fri_ros2/include/lbr_fri_ros2/formatting.hpp b/lbr_fri_ros2/include/lbr_fri_ros2/formatting.hpp index debb2e86..5fe5ab32 100644 --- a/lbr_fri_ros2/include/lbr_fri_ros2/formatting.hpp +++ b/lbr_fri_ros2/include/lbr_fri_ros2/formatting.hpp @@ -76,5 +76,5 @@ struct EnumMaps { } }; }; -} // end of namespace lbr_fri_ros2 +} // namespace lbr_fri_ros2 #endif // LBR_FRI_ROS2__ENUM_MAPS_HPP diff --git a/lbr_fri_ros2/include/lbr_fri_ros2/ft_estimator.hpp b/lbr_fri_ros2/include/lbr_fri_ros2/ft_estimator.hpp index 57c81292..b4257a9e 100644 --- a/lbr_fri_ros2/include/lbr_fri_ros2/ft_estimator.hpp +++ b/lbr_fri_ros2/include/lbr_fri_ros2/ft_estimator.hpp @@ -66,5 +66,5 @@ class FTEstimator { Eigen::Matrix tau_ext_; Eigen::Matrix f_ext_; }; -} // end of namespace lbr_fri_ros2 +} // namespace lbr_fri_ros2 #endif // LBR_FRI_ROS2__FT_ESTIMATOR_HPP_ diff --git a/lbr_fri_ros2/include/lbr_fri_ros2/interfaces/position_command.hpp b/lbr_fri_ros2/include/lbr_fri_ros2/interfaces/position_command.hpp index d2f345c5..24e8a324 100644 --- a/lbr_fri_ros2/include/lbr_fri_ros2/interfaces/position_command.hpp +++ b/lbr_fri_ros2/include/lbr_fri_ros2/interfaces/position_command.hpp @@ -18,5 +18,5 @@ class PositionCommandInterface : public BaseCommandInterface { void buffered_command_to_fri(fri_command_t_ref command, const_idl_state_t_ref state) override; }; -} // end of namespace lbr_fri_ros2 +} // namespace lbr_fri_ros2 #endif // LBR_FRI_ROS2__INTERFACES__POSITION_COMMAND_HPP_ diff --git a/lbr_fri_ros2/include/lbr_fri_ros2/interfaces/state.hpp b/lbr_fri_ros2/include/lbr_fri_ros2/interfaces/state.hpp index cdfc97a7..dd4545fa 100644 --- a/lbr_fri_ros2/include/lbr_fri_ros2/interfaces/state.hpp +++ b/lbr_fri_ros2/include/lbr_fri_ros2/interfaces/state.hpp @@ -55,5 +55,5 @@ class StateInterface { StateInterfaceParameters parameters_; JointExponentialFilterArray external_torque_filter_, measured_torque_filter_; }; -} // end of namespace lbr_fri_ros2 +} // namespace lbr_fri_ros2 #endif // LBR_FRI_ROS2__INTERFACES__STATE_HPP_ diff --git a/lbr_fri_ros2/include/lbr_fri_ros2/interfaces/torque_command.hpp b/lbr_fri_ros2/include/lbr_fri_ros2/interfaces/torque_command.hpp index fab800eb..e4ec7874 100644 --- a/lbr_fri_ros2/include/lbr_fri_ros2/interfaces/torque_command.hpp +++ b/lbr_fri_ros2/include/lbr_fri_ros2/interfaces/torque_command.hpp @@ -18,5 +18,5 @@ class TorqueCommandInterface : public BaseCommandInterface { void buffered_command_to_fri(fri_command_t_ref command, const_idl_state_t_ref state) override; }; -} // end of namespace lbr_fri_ros2 +} // namespace lbr_fri_ros2 #endif // LBR_FRI_ROS2__INTERFACES__TORQUE_COMMAND_HPP_ diff --git a/lbr_fri_ros2/include/lbr_fri_ros2/interfaces/wrench_command.hpp b/lbr_fri_ros2/include/lbr_fri_ros2/interfaces/wrench_command.hpp index 69b55b1a..0c846ee4 100644 --- a/lbr_fri_ros2/include/lbr_fri_ros2/interfaces/wrench_command.hpp +++ b/lbr_fri_ros2/include/lbr_fri_ros2/interfaces/wrench_command.hpp @@ -18,5 +18,5 @@ class WrenchCommandInterface : public BaseCommandInterface { void buffered_command_to_fri(fri_command_t_ref command, const_idl_state_t_ref state) override; }; -} // end of namespace lbr_fri_ros2 +} // namespace lbr_fri_ros2 #endif // LBR_FRI_ROS2__INTERFACES__WRENCH_COMMAND_HPP_ diff --git a/lbr_fri_ros2/include/lbr_fri_ros2/pinv.hpp b/lbr_fri_ros2/include/lbr_fri_ros2/pinv.hpp index 1616c27e..8d5d0b16 100644 --- a/lbr_fri_ros2/include/lbr_fri_ros2/pinv.hpp +++ b/lbr_fri_ros2/include/lbr_fri_ros2/pinv.hpp @@ -25,5 +25,5 @@ pinv(const MatT &mat, }); return svd.matrixV() * dampedSingularValuesInv * svd.matrixU().adjoint(); } -} // end of namespace lbr_fri_ros2 +} // namespace lbr_fri_ros2 #endif // LBR_FRI_ROS2__PINV_HPP_ diff --git a/lbr_fri_ros2/src/app.cpp b/lbr_fri_ros2/src/app.cpp index 54abde3f..7642a814 100644 --- a/lbr_fri_ros2/src/app.cpp +++ b/lbr_fri_ros2/src/app.cpp @@ -132,4 +132,4 @@ bool App::valid_port_(const int &port_id) { } return true; } -} // end of namespace lbr_fri_ros2 +} // namespace lbr_fri_ros2 diff --git a/lbr_fri_ros2/src/async_client.cpp b/lbr_fri_ros2/src/async_client.cpp index 5c572fcc..672dfc7a 100644 --- a/lbr_fri_ros2/src/async_client.cpp +++ b/lbr_fri_ros2/src/async_client.cpp @@ -91,4 +91,4 @@ void AsyncClient::command() { state_interface_ptr_->get_state()); // current state accessed via state interface (allows for // open loop and is statically sized) } -} // end of namespace lbr_fri_ros2 +} // namespace lbr_fri_ros2 diff --git a/lbr_fri_ros2/src/command_guard.cpp b/lbr_fri_ros2/src/command_guard.cpp index 8ba1e04a..92d06e5b 100644 --- a/lbr_fri_ros2/src/command_guard.cpp +++ b/lbr_fri_ros2/src/command_guard.cpp @@ -110,4 +110,4 @@ command_guard_factory(const CommandGuardParameters &command_guard_parameters, ColorScheme::ERROR << err.c_str() << ColorScheme::ENDC); throw std::runtime_error(err); } -} // end of namespace lbr_fri_ros2 +} // namespace lbr_fri_ros2 diff --git a/lbr_fri_ros2/src/filters.cpp b/lbr_fri_ros2/src/filters.cpp index f2e98052..4047e59a 100644 --- a/lbr_fri_ros2/src/filters.cpp +++ b/lbr_fri_ros2/src/filters.cpp @@ -80,4 +80,4 @@ void JointPIDArray::log_info() const { RCLCPP_INFO(rclcpp::get_logger(LOGGER_NAME), "* antiwindup: %s", pid_parameters_.antiwindup ? "true" : "false"); }; -} // end of namespace lbr_fri_ros2 +} // namespace lbr_fri_ros2 diff --git a/lbr_fri_ros2/src/ft_estimator.cpp b/lbr_fri_ros2/src/ft_estimator.cpp index 35929be8..8ac0981c 100644 --- a/lbr_fri_ros2/src/ft_estimator.cpp +++ b/lbr_fri_ros2/src/ft_estimator.cpp @@ -56,4 +56,4 @@ void FTEstimator::reset() { tau_ext_.setZero(); f_ext_.setZero(); } -} // end of namespace lbr_fri_ros2 +} // namespace lbr_fri_ros2 diff --git a/lbr_fri_ros2/src/interfaces/position_command.cpp b/lbr_fri_ros2/src/interfaces/position_command.cpp index 26287bdf..1b5a33da 100644 --- a/lbr_fri_ros2/src/interfaces/position_command.cpp +++ b/lbr_fri_ros2/src/interfaces/position_command.cpp @@ -57,4 +57,4 @@ void PositionCommandInterface::buffered_command_to_fri(fri_command_t_ref command // write joint position to output command.setJointPosition(command_.joint_position.data()); } -} // end of namespace lbr_fri_ros2 +} // namespace lbr_fri_ros2 diff --git a/lbr_fri_ros2/src/interfaces/torque_command.cpp b/lbr_fri_ros2/src/interfaces/torque_command.cpp index f731fc9b..b658b266 100644 --- a/lbr_fri_ros2/src/interfaces/torque_command.cpp +++ b/lbr_fri_ros2/src/interfaces/torque_command.cpp @@ -48,4 +48,4 @@ void TorqueCommandInterface::buffered_command_to_fri(fri_command_t_ref command, command.setJointPosition(command_.joint_position.data()); command.setTorque(command_.torque.data()); } -} // end of namespace lbr_fri_ros2 +} // namespace lbr_fri_ros2 diff --git a/lbr_fri_ros2/src/interfaces/wrench_command.cpp b/lbr_fri_ros2/src/interfaces/wrench_command.cpp index 9ab34524..9dd9618a 100644 --- a/lbr_fri_ros2/src/interfaces/wrench_command.cpp +++ b/lbr_fri_ros2/src/interfaces/wrench_command.cpp @@ -47,4 +47,4 @@ void WrenchCommandInterface::buffered_command_to_fri(fri_command_t_ref command, command.setJointPosition(command_.joint_position.data()); command.setWrench(command_.wrench.data()); } -} // end of namespace lbr_fri_ros2 +} // namespace lbr_fri_ros2 diff --git a/lbr_ros2_control/CMakeLists.txt b/lbr_ros2_control/CMakeLists.txt index 078cd0bf..6dc55918 100644 --- a/lbr_ros2_control/CMakeLists.txt +++ b/lbr_ros2_control/CMakeLists.txt @@ -29,10 +29,10 @@ find_package(realtime_tools REQUIRED) add_library( ${PROJECT_NAME} SHARED - src/lbr_joint_position_command_controller.cpp - src/lbr_torque_command_controller.cpp - src/lbr_wrench_command_controller.cpp - src/lbr_state_broadcaster.cpp + src/controllers/lbr_joint_position_command_controller.cpp + src/controllers/lbr_torque_command_controller.cpp + src/controllers/lbr_wrench_command_controller.cpp + src/controllers/lbr_state_broadcaster.cpp src/system_interface.cpp ) @@ -62,8 +62,7 @@ target_link_libraries(${PROJECT_NAME} FRIClient::FRIClient ) -pluginlib_export_plugin_description_file(controller_interface plugin_description_files/broadcasters.xml) -pluginlib_export_plugin_description_file(controller_interface plugin_description_files/forward_command_controllers.xml) +pluginlib_export_plugin_description_file(controller_interface plugin_description_files/controllers.xml) pluginlib_export_plugin_description_file(hardware_interface plugin_description_files/system_interface.xml) # Export for downstream usage, see https://docs.ros.org/en/foxy/Guides/Ament-CMake-Documentation.html diff --git a/lbr_ros2_control/config/lbr_controllers.yaml b/lbr_ros2_control/config/lbr_controllers.yaml index a9e70793..c7939d25 100644 --- a/lbr_ros2_control/config/lbr_controllers.yaml +++ b/lbr_ros2_control/config/lbr_controllers.yaml @@ -2,7 +2,7 @@ # managers regardless of their namespace. Usefull in multi-robot setups. /**/controller_manager: ros__parameters: - update_rate: 100 + update_rate: 200 # ROS 2 control broadcasters joint_state_broadcaster: @@ -32,6 +32,10 @@ lbr_wrench_command_controller: type: lbr_ros2_control/LBRWrenchCommandController + # Admittance controller + admittance_controller: + type: lbr_ros2_control/AdmittanceController + /**/force_torque_broadcaster: ros__parameters: frame_id: lbr/link_ee # namespace: https://github.com/ros2/rviz/issues/1103 diff --git a/lbr_ros2_control/include/lbr_ros2_control/controllers/admittance_controller.hpp b/lbr_ros2_control/include/lbr_ros2_control/controllers/admittance_controller.hpp new file mode 100644 index 00000000..6d28e3a1 --- /dev/null +++ b/lbr_ros2_control/include/lbr_ros2_control/controllers/admittance_controller.hpp @@ -0,0 +1,63 @@ +#ifndef LBR_ROS2_CONTROL__ADMITTANCE_CONTROLLER_HPP_ +#define LBR_ROS2_CONTROL__ADMITTANCE_CONTROLLER_HPP_ + +#include +#include +#include +#include +#include +#include + +#include "controller_interface/controller_interface.hpp" +#include "hardware_interface/loaned_command_interface.hpp" +#include "hardware_interface/types/hardware_interface_type_values.hpp" +#include "rclcpp/rclcpp.hpp" + +#include "friLBRState.h" +#include "lbr_ros2_control/system_interface_type_values.hpp" + +namespace lbr_ros2_control { +class Admittance { + +} + +class AdmittanceController : public controller_interface::ControllerInterface { + static constexpr uint8_t CARTESIAN_DOF = 6; + +public: + AdmittanceController(); + + controller_interface::InterfaceConfiguration command_interface_configuration() const override; + + controller_interface::InterfaceConfiguration state_interface_configuration() const override; + + controller_interface::CallbackReturn on_init() override; + + controller_interface::return_type update(const rclcpp::Time &time, + const rclcpp::Duration &period) override; + + controller_interface::CallbackReturn + on_configure(const rclcpp_lifecycle::State &previous_state) override; + + controller_interface::CallbackReturn + on_activate(const rclcpp_lifecycle::State &previous_state) override; + + controller_interface::CallbackReturn + on_deactivate(const rclcpp_lifecycle::State &previous_state) override; + +protected: + bool reference_command_interfaces_(); + bool reference_state_interfaces_(); + void clear_command_interfaces_(); + void clear_state_interfaces_(); + + std::array joint_names_ = { + "A1", "A2", "A3", "A4", "A5", "A6", "A7"}; + + std::vector> + joint_position_command_interfaces_; + std::vector> + estimated_ft_sensor_state_interface_; +}; +} // namespace lbr_ros2_control +#endif // LBR_ROS2_CONTROL__ADMITTANCE_CONTROLLER_HPP_ diff --git a/lbr_ros2_control/include/lbr_ros2_control/lbr_joint_position_command_controller.hpp b/lbr_ros2_control/include/lbr_ros2_control/controllers/lbr_joint_position_command_controller.hpp similarity index 98% rename from lbr_ros2_control/include/lbr_ros2_control/lbr_joint_position_command_controller.hpp rename to lbr_ros2_control/include/lbr_ros2_control/controllers/lbr_joint_position_command_controller.hpp index 5749b9f4..46eebecd 100644 --- a/lbr_ros2_control/include/lbr_ros2_control/lbr_joint_position_command_controller.hpp +++ b/lbr_ros2_control/include/lbr_ros2_control/controllers/lbr_joint_position_command_controller.hpp @@ -48,5 +48,5 @@ class LBRJointPositionCommandController : public controller_interface::Controlle rclcpp::Subscription::SharedPtr lbr_joint_position_command_subscription_ptr_; }; -} // end of namespace lbr_ros2_control +} // namespace lbr_ros2_control #endif // LBR_ROS2_CONTROL__LBR_JOINT_POSITION_COMMAND_CONTROLLER_HPP_ diff --git a/lbr_ros2_control/include/lbr_ros2_control/lbr_state_broadcaster.hpp b/lbr_ros2_control/include/lbr_ros2_control/controllers/lbr_state_broadcaster.hpp similarity index 98% rename from lbr_ros2_control/include/lbr_ros2_control/lbr_state_broadcaster.hpp rename to lbr_ros2_control/include/lbr_ros2_control/controllers/lbr_state_broadcaster.hpp index 8df3df4d..a872a03f 100644 --- a/lbr_ros2_control/include/lbr_ros2_control/lbr_state_broadcaster.hpp +++ b/lbr_ros2_control/include/lbr_ros2_control/controllers/lbr_state_broadcaster.hpp @@ -56,5 +56,5 @@ class LBRStateBroadcaster : public controller_interface::ControllerInterface { std::shared_ptr> rt_state_publisher_ptr_; }; -} // end of namespace lbr_ros2_control +} // namespace lbr_ros2_control #endif // LBR_ROS2_CONTROL__LBR_STATE_BROADCASTER_HPP_ diff --git a/lbr_ros2_control/include/lbr_ros2_control/lbr_torque_command_controller.hpp b/lbr_ros2_control/include/lbr_ros2_control/controllers/lbr_torque_command_controller.hpp similarity index 98% rename from lbr_ros2_control/include/lbr_ros2_control/lbr_torque_command_controller.hpp rename to lbr_ros2_control/include/lbr_ros2_control/controllers/lbr_torque_command_controller.hpp index f86d1c25..7a4c5f7b 100644 --- a/lbr_ros2_control/include/lbr_ros2_control/lbr_torque_command_controller.hpp +++ b/lbr_ros2_control/include/lbr_ros2_control/controllers/lbr_torque_command_controller.hpp @@ -56,5 +56,5 @@ class LBRTorqueCommandController : public controller_interface::ControllerInterf rclcpp::Subscription::SharedPtr lbr_torque_command_subscription_ptr_; }; -} // end of namespace lbr_ros2_control +} // namespace lbr_ros2_control #endif // LBR_ROS2_CONTROL__LBR_TORQUE_COMMAND_CONTROLLER_HPP_ diff --git a/lbr_ros2_control/include/lbr_ros2_control/lbr_wrench_command_controller.hpp b/lbr_ros2_control/include/lbr_ros2_control/controllers/lbr_wrench_command_controller.hpp similarity index 98% rename from lbr_ros2_control/include/lbr_ros2_control/lbr_wrench_command_controller.hpp rename to lbr_ros2_control/include/lbr_ros2_control/controllers/lbr_wrench_command_controller.hpp index 183e4cf4..52b76f35 100644 --- a/lbr_ros2_control/include/lbr_ros2_control/lbr_wrench_command_controller.hpp +++ b/lbr_ros2_control/include/lbr_ros2_control/controllers/lbr_wrench_command_controller.hpp @@ -59,5 +59,5 @@ class LBRWrenchCommandController : public controller_interface::ControllerInterf rclcpp::Subscription::SharedPtr lbr_wrench_command_subscription_ptr_; }; -} // end of namespace lbr_ros2_control +} // namespace lbr_ros2_control #endif // LBR_ROS2_CONTROL__LBR_WRENCH_COMMAND_CONTROLLER_HPP_ diff --git a/lbr_ros2_control/include/lbr_ros2_control/system_interface.hpp b/lbr_ros2_control/include/lbr_ros2_control/system_interface.hpp index aace5b6a..99d1f0a4 100644 --- a/lbr_ros2_control/include/lbr_ros2_control/system_interface.hpp +++ b/lbr_ros2_control/include/lbr_ros2_control/system_interface.hpp @@ -166,5 +166,5 @@ class SystemInterface : public hardware_interface::SystemInterface { // exposed command interfaces lbr_fri_idl::msg::LBRCommand hw_lbr_command_; }; -} // end of namespace lbr_ros2_control +} // namespace lbr_ros2_control #endif // LBR_ROS2_CONTROL__SYSTEM_INTERFACE_HPP_ diff --git a/lbr_ros2_control/include/lbr_ros2_control/system_interface_type_values.hpp b/lbr_ros2_control/include/lbr_ros2_control/system_interface_type_values.hpp index 819e196f..bb750426 100644 --- a/lbr_ros2_control/include/lbr_ros2_control/system_interface_type_values.hpp +++ b/lbr_ros2_control/include/lbr_ros2_control/system_interface_type_values.hpp @@ -38,5 +38,5 @@ constexpr char HW_IF_TORQUE_Z[] = "torque.z"; constexpr char HW_IF_WRENCH_PREFIX[] = "wrench"; constexpr char HW_IF_AUXILIARY_PREFIX[] = "auxiliary_sensor"; constexpr char HW_IF_ESTIMATED_FT_PREFIX[] = "estimated_ft_sensor"; -} // end of namespace lbr_ros2_control +} // namespace lbr_ros2_control #endif // LBR_ROS2_CONTROL__SYSTEM_INTERFACE_TYPE_VALUES_HPP_ diff --git a/lbr_ros2_control/plugin_description_files/broadcasters.xml b/lbr_ros2_control/plugin_description_files/broadcasters.xml deleted file mode 100644 index 0c2484f0..00000000 --- a/lbr_ros2_control/plugin_description_files/broadcasters.xml +++ /dev/null @@ -1,8 +0,0 @@ - - - - - Broadcaster for LBRState messages, see lbr_fri_idl/msg/LBRState.msg. - - \ No newline at end of file diff --git a/lbr_ros2_control/plugin_description_files/forward_command_controllers.xml b/lbr_ros2_control/plugin_description_files/controllers.xml similarity index 67% rename from lbr_ros2_control/plugin_description_files/forward_command_controllers.xml rename to lbr_ros2_control/plugin_description_files/controllers.xml index b5185800..82c4ae8e 100644 --- a/lbr_ros2_control/plugin_description_files/forward_command_controllers.xml +++ b/lbr_ros2_control/plugin_description_files/controllers.xml @@ -1,5 +1,11 @@ + + + Broadcaster for LBRState messages, see lbr_fri_idl/msg/LBRState.msg. + + Forward command controller for LBRWrenchCommand message, see lbr_fri_idl/msg/LBRWrenchCommand.msg. + + + + A simple admittance controller. + \ No newline at end of file diff --git a/lbr_ros2_control/src/controllers/admittance_controller.cpp b/lbr_ros2_control/src/controllers/admittance_controller.cpp new file mode 100644 index 00000000..147ad742 --- /dev/null +++ b/lbr_ros2_control/src/controllers/admittance_controller.cpp @@ -0,0 +1,77 @@ +#include "lbr_ros2_control/controllers/admittance_controller.hpp" + +namespace lbr_ros2_control { +AdmittanceController::AdmittanceController() {} + +controller_interface::InterfaceConfiguration +AdmittanceController::command_interface_configuration() { + // reference joint position command interface +} + +controller_interface::InterfaceConfiguration AdmittanceController::state_interface_configuration() { + // retrieve estimated ft state interface +} + +controller_interface::CallbackReturn AdmittanceController::on_init() {} + +controller_interface::return_type AdmittanceController::update(const rclcpp::Time &time, + const rclcpp::Duration &period) { + // compute admittance + // add warning for high force-torques and refer to load data calibration +} + +controller_interface::CallbackReturn +AdmittanceController::on_configure(const rclcpp_lifecycle::State &previous_state) { + return controller_interface::CallbackReturn::SUCCESS; +} + +controller_interface::CallbackReturn +AdmittanceController::on_activate(const rclcpp_lifecycle::State &previous_state) { + if (!reference_command_interfaces_()) { + return controller_interface::CallbackReturn::ERROR; + } + if (!reference_state_interfaces_()) { + return controller_interface::CallbackReturn::ERROR; + } + return controller_interface::CallbackReturn::SUCCESS; +} + +controller_interface::CallbackReturn +AdmittanceController::on_deactivate(const rclcpp_lifecycle::State &previous_state) { + clear_command_interfaces_(); + clear_state_interfaces_(); + return controller_interface::CallbackReturn::SUCCESS; +} + +bool AdmittanceController::reference_command_interfaces_() { + for (auto &command_interface : command_interfaces_) { + if (command_interface.get_interface_name() == hardware_interface::HW_IF_POSITION) { + joint_position_command_interfaces_.emplace_back(std::ref(command_interface)); + } + } + if (joint_position_command_interfaces_.size() != KUKA::FRI::LBRState::NUMBER_OF_JOINTS) { + RCLCPP_ERROR( + this->get_node()->get_logger(), + "Number of joint position command interfaces '%ld' does not match the number of joints " + "in the robot '%d'.", + joint_position_command_interfaces_.size(), KUKA::FRI::LBRState::NUMBER_OF_JOINTS); + return false; + } +} + +bool AdmittanceController::reference_state_interfaces_() { + for (auto &state_interface : state_interfaces_) { + if (state_interface.get_interface_name() == HW_IF_ESTIMATED_FT_PREFIX) { + estimated_ft_sensor_state_interface_.emplace_back(std::ref(state_interface)); + } + } +} + +void AdmittanceController::clear_command_interfaces_() { + joint_position_command_interfaces_.clear(); +} + +void AdmittanceController::clear_state_interfaces_() { + estimated_ft_sensor_state_interface_.clear(); +} +} // namespace lbr_ros2_control diff --git a/lbr_ros2_control/src/lbr_joint_position_command_controller.cpp b/lbr_ros2_control/src/controllers/lbr_joint_position_command_controller.cpp similarity index 96% rename from lbr_ros2_control/src/lbr_joint_position_command_controller.cpp rename to lbr_ros2_control/src/controllers/lbr_joint_position_command_controller.cpp index 46fb0170..f5217b82 100644 --- a/lbr_ros2_control/src/lbr_joint_position_command_controller.cpp +++ b/lbr_ros2_control/src/controllers/lbr_joint_position_command_controller.cpp @@ -1,4 +1,4 @@ -#include "lbr_ros2_control/lbr_joint_position_command_controller.hpp" +#include "lbr_ros2_control/controllers/lbr_joint_position_command_controller.hpp" namespace lbr_ros2_control { LBRJointPositionCommandController::LBRJointPositionCommandController() @@ -66,7 +66,7 @@ controller_interface::CallbackReturn LBRJointPositionCommandController::on_deact const rclcpp_lifecycle::State & /*previous_state*/) { return controller_interface::CallbackReturn::SUCCESS; } -} // end of namespace lbr_ros2_control +} // namespace lbr_ros2_control #include "pluginlib/class_list_macros.hpp" diff --git a/lbr_ros2_control/src/lbr_state_broadcaster.cpp b/lbr_ros2_control/src/controllers/lbr_state_broadcaster.cpp similarity index 98% rename from lbr_ros2_control/src/lbr_state_broadcaster.cpp rename to lbr_ros2_control/src/controllers/lbr_state_broadcaster.cpp index 3ea4afaf..4a3621af 100644 --- a/lbr_ros2_control/src/lbr_state_broadcaster.cpp +++ b/lbr_ros2_control/src/controllers/lbr_state_broadcaster.cpp @@ -1,4 +1,4 @@ -#include "lbr_ros2_control/lbr_state_broadcaster.hpp" +#include "lbr_ros2_control/controllers/lbr_state_broadcaster.hpp" namespace lbr_ros2_control { controller_interface::InterfaceConfiguration @@ -154,7 +154,7 @@ void LBRStateBroadcaster::init_state_msg_() { rt_state_publisher_ptr_->msg_.time_stamp_sec = std::numeric_limits::quiet_NaN(); rt_state_publisher_ptr_->msg_.tracking_performance = std::numeric_limits::quiet_NaN(); } -} // end of namespace lbr_ros2_control +} // namespace lbr_ros2_control #include "pluginlib/class_list_macros.hpp" diff --git a/lbr_ros2_control/src/lbr_torque_command_controller.cpp b/lbr_ros2_control/src/controllers/lbr_torque_command_controller.cpp similarity index 97% rename from lbr_ros2_control/src/lbr_torque_command_controller.cpp rename to lbr_ros2_control/src/controllers/lbr_torque_command_controller.cpp index 2892f8eb..a4f674f6 100644 --- a/lbr_ros2_control/src/lbr_torque_command_controller.cpp +++ b/lbr_ros2_control/src/controllers/lbr_torque_command_controller.cpp @@ -1,4 +1,4 @@ -#include "lbr_ros2_control/lbr_torque_command_controller.hpp" +#include "lbr_ros2_control/controllers/lbr_torque_command_controller.hpp" namespace lbr_ros2_control { LBRTorqueCommandController::LBRTorqueCommandController() @@ -102,7 +102,7 @@ void LBRTorqueCommandController::clear_command_interfaces_() { joint_position_command_interfaces_.clear(); torque_command_interfaces_.clear(); } -} // end of namespace lbr_ros2_control +} // namespace lbr_ros2_control #include "pluginlib/class_list_macros.hpp" diff --git a/lbr_ros2_control/src/lbr_wrench_command_controller.cpp b/lbr_ros2_control/src/controllers/lbr_wrench_command_controller.cpp similarity index 97% rename from lbr_ros2_control/src/lbr_wrench_command_controller.cpp rename to lbr_ros2_control/src/controllers/lbr_wrench_command_controller.cpp index 4f00a986..53445f1f 100644 --- a/lbr_ros2_control/src/lbr_wrench_command_controller.cpp +++ b/lbr_ros2_control/src/controllers/lbr_wrench_command_controller.cpp @@ -1,4 +1,4 @@ -#include "lbr_ros2_control/lbr_wrench_command_controller.hpp" +#include "lbr_ros2_control/controllers/lbr_wrench_command_controller.hpp" namespace lbr_ros2_control { LBRWrenchCommandController::LBRWrenchCommandController() @@ -108,7 +108,7 @@ void LBRWrenchCommandController::clear_command_interfaces_() { joint_position_command_interfaces_.clear(); wrench_command_interfaces_.clear(); } -} // end of namespace lbr_ros2_control +} // namespace lbr_ros2_control #include "pluginlib/class_list_macros.hpp" diff --git a/lbr_ros2_control/src/system_interface.cpp b/lbr_ros2_control/src/system_interface.cpp index 517818d0..3bbaa9d1 100644 --- a/lbr_ros2_control/src/system_interface.cpp +++ b/lbr_ros2_control/src/system_interface.cpp @@ -683,7 +683,7 @@ void SystemInterface::compute_hw_velocity_() { }); } -} // end of namespace lbr_ros2_control +} // namespace lbr_ros2_control #include From ae9373e0f916be6d1d299699ed095be1698485c9 Mon Sep 17 00:00:00 2001 From: mhubii Date: Tue, 16 Jul 2024 14:09:10 +0200 Subject: [PATCH 02/15] removed launch_mixins --- .../{launch_mixins/__init__.py => launch/fake_hardware.launch.py} | 0 lbr_bringup/launch/{sim.launch.py => gazebo.launch.py} | 0 lbr_bringup/launch/moveit_servo.launch.py | 0 lbr_bringup/lbr_bringup/__init__.py | 0 lbr_bringup/{launch_mixins => lbr_bringup}/lbr_bringup.py | 0 lbr_bringup/{launch_mixins => lbr_bringup}/lbr_description.py | 0 lbr_bringup/{launch_mixins => lbr_bringup}/lbr_ros2_control.py | 0 7 files changed, 0 insertions(+), 0 deletions(-) rename lbr_bringup/{launch_mixins/__init__.py => launch/fake_hardware.launch.py} (100%) rename lbr_bringup/launch/{sim.launch.py => gazebo.launch.py} (100%) create mode 100644 lbr_bringup/launch/moveit_servo.launch.py create mode 100644 lbr_bringup/lbr_bringup/__init__.py rename lbr_bringup/{launch_mixins => lbr_bringup}/lbr_bringup.py (100%) rename lbr_bringup/{launch_mixins => lbr_bringup}/lbr_description.py (100%) rename lbr_bringup/{launch_mixins => lbr_bringup}/lbr_ros2_control.py (100%) diff --git a/lbr_bringup/launch_mixins/__init__.py b/lbr_bringup/launch/fake_hardware.launch.py similarity index 100% rename from lbr_bringup/launch_mixins/__init__.py rename to lbr_bringup/launch/fake_hardware.launch.py diff --git a/lbr_bringup/launch/sim.launch.py b/lbr_bringup/launch/gazebo.launch.py similarity index 100% rename from lbr_bringup/launch/sim.launch.py rename to lbr_bringup/launch/gazebo.launch.py diff --git a/lbr_bringup/launch/moveit_servo.launch.py b/lbr_bringup/launch/moveit_servo.launch.py new file mode 100644 index 00000000..e69de29b diff --git a/lbr_bringup/lbr_bringup/__init__.py b/lbr_bringup/lbr_bringup/__init__.py new file mode 100644 index 00000000..e69de29b diff --git a/lbr_bringup/launch_mixins/lbr_bringup.py b/lbr_bringup/lbr_bringup/lbr_bringup.py similarity index 100% rename from lbr_bringup/launch_mixins/lbr_bringup.py rename to lbr_bringup/lbr_bringup/lbr_bringup.py diff --git a/lbr_bringup/launch_mixins/lbr_description.py b/lbr_bringup/lbr_bringup/lbr_description.py similarity index 100% rename from lbr_bringup/launch_mixins/lbr_description.py rename to lbr_bringup/lbr_bringup/lbr_description.py diff --git a/lbr_bringup/launch_mixins/lbr_ros2_control.py b/lbr_bringup/lbr_bringup/lbr_ros2_control.py similarity index 100% rename from lbr_bringup/launch_mixins/lbr_ros2_control.py rename to lbr_bringup/lbr_bringup/lbr_ros2_control.py From 4e5175f468ca72c432b1769b3282da5cdc98cdf7 Mon Sep 17 00:00:00 2001 From: mhubii Date: Mon, 9 Sep 2024 19:03:29 +0200 Subject: [PATCH 03/15] initial lego launch re-factor --- lbr_bringup/CMakeLists.txt | 2 +- lbr_bringup/launch/bringup.launch.py | 31 +++- lbr_bringup/launch/gazebo.launch.py | 159 ++++------------ lbr_bringup/launch/hardware.launch.py | 68 +++++++ lbr_bringup/launch/mock.launch.py | 59 ++++++ lbr_bringup/launch/move_group.launch.py | 36 ++-- lbr_bringup/launch/real.launch.py | 171 ------------------ ...ake_hardware.launch.py => rviz2.launch.py} | 0 lbr_bringup/launch/system_interface.launch.py | 53 ------ .../{lbr_description.py => description.py} | 25 +-- .../{lbr_bringup.py => move_group.py} | 0 .../{lbr_ros2_control.py => ros2_control.py} | 5 +- lbr_description/urdf/iiwa14/iiwa14.xacro | 4 +- .../urdf/iiwa14/iiwa14_description.xacro | 6 +- lbr_description/urdf/iiwa7/iiwa7.xacro | 4 +- .../urdf/iiwa7/iiwa7_description.xacro | 6 +- lbr_description/urdf/med14/med14.xacro | 4 +- .../urdf/med14/med14_description.xacro | 6 +- lbr_description/urdf/med7/med7.xacro | 4 +- .../urdf/med7/med7_description.xacro | 6 +- .../config/lbr_system_interface.xacro | 47 ++--- 21 files changed, 260 insertions(+), 436 deletions(-) create mode 100644 lbr_bringup/launch/hardware.launch.py create mode 100644 lbr_bringup/launch/mock.launch.py delete mode 100644 lbr_bringup/launch/real.launch.py rename lbr_bringup/launch/{fake_hardware.launch.py => rviz2.launch.py} (100%) delete mode 100644 lbr_bringup/launch/system_interface.launch.py rename lbr_bringup/lbr_bringup/{lbr_description.py => description.py} (91%) rename lbr_bringup/lbr_bringup/{lbr_bringup.py => move_group.py} (100%) rename lbr_bringup/lbr_bringup/{lbr_ros2_control.py => ros2_control.py} (95%) diff --git a/lbr_bringup/CMakeLists.txt b/lbr_bringup/CMakeLists.txt index cd7e1288..54f14a29 100644 --- a/lbr_bringup/CMakeLists.txt +++ b/lbr_bringup/CMakeLists.txt @@ -15,6 +15,6 @@ install( ) # Launch mixins -ament_python_install_package(launch_mixins) +ament_python_install_package(lbr_bringup) ament_package() diff --git a/lbr_bringup/launch/bringup.launch.py b/lbr_bringup/launch/bringup.launch.py index b4fb2420..8ae5f8a4 100644 --- a/lbr_bringup/launch/bringup.launch.py +++ b/lbr_bringup/launch/bringup.launch.py @@ -1,16 +1,16 @@ from launch import LaunchDescription from launch.actions import IncludeLaunchDescription -from launch.conditions import IfCondition, UnlessCondition +from launch.conditions import LaunchConfigurationEquals from launch.launch_description_sources import PythonLaunchDescriptionSource from launch.substitutions import LaunchConfiguration, PathJoinSubstitution -from launch_mixins.lbr_description import LBRDescriptionMixin from launch_ros.substitutions import FindPackageShare +from lbr_bringup.description import LBRDescriptionMixin def generate_launch_description() -> LaunchDescription: ld = LaunchDescription() - ld.add_action(LBRDescriptionMixin.arg_sim()) + ld.add_action(LBRDescriptionMixin.arg_mode()) ld.add_action( IncludeLaunchDescription( @@ -19,11 +19,28 @@ def generate_launch_description() -> LaunchDescription: [ FindPackageShare("lbr_bringup"), "launch", - "sim.launch.py", + "hardware.launch.py", ] ) ), - condition=IfCondition(LaunchConfiguration("sim")), + condition=LaunchConfigurationEquals( + LaunchConfiguration("mode"), "hardware" + ), + ) + ) + + ld.add_action( + IncludeLaunchDescription( + PythonLaunchDescriptionSource( + PathJoinSubstitution( + [ + FindPackageShare("lbr_bringup"), + "launch", + "mock.launch.py", + ] + ) + ), + condition=LaunchConfigurationEquals(LaunchConfiguration("mode"), "mock"), ) ) @@ -34,11 +51,11 @@ def generate_launch_description() -> LaunchDescription: [ FindPackageShare("lbr_bringup"), "launch", - "real.launch.py", + "gazebo.launch.py", ] ) ), - condition=UnlessCondition(LaunchConfiguration("sim")), + condition=LaunchConfigurationEquals(LaunchConfiguration("mode"), "gazebo"), ) ) diff --git a/lbr_bringup/launch/gazebo.launch.py b/lbr_bringup/launch/gazebo.launch.py index edac639d..ee76f9b3 100644 --- a/lbr_bringup/launch/gazebo.launch.py +++ b/lbr_bringup/launch/gazebo.launch.py @@ -1,150 +1,53 @@ -from typing import List +from launch import LaunchDescription +from launch.substitutions import LaunchConfiguration, PathJoinSubstitution +from lbr_bringup.description import GazeboMixin, LBRDescriptionMixin +from lbr_bringup.ros2_control import LBRROS2ControlMixin -from launch import LaunchContext, LaunchDescription, LaunchDescriptionEntity -from launch.actions import DeclareLaunchArgument, OpaqueFunction, RegisterEventHandler -from launch.conditions import IfCondition -from launch.event_handlers import OnProcessExit -from launch.substitutions import ( - AndSubstitution, - LaunchConfiguration, - NotSubstitution, - PathJoinSubstitution, -) -from launch_mixins.lbr_bringup import LBRMoveGroupMixin -from launch_mixins.lbr_description import GazeboMixin, LBRDescriptionMixin, RVizMixin -from launch_mixins.lbr_ros2_control import LBRROS2ControlMixin - -def launch_setup(context: LaunchContext) -> List[LaunchDescriptionEntity]: +def generate_launch_description() -> LaunchDescription: ld = LaunchDescription() - robot_description = LBRDescriptionMixin.param_robot_description(sim=True) - world_robot_tf = [0, 0, 0, 0, 0, 0] # keep zero - ld.add_action(GazeboMixin.include_gazebo()) # Gazebo has its own controller manager - spawn_entity = GazeboMixin.node_spawn_entity(tf=world_robot_tf) - ld.add_action(spawn_entity) - joint_state_broadcaster = LBRROS2ControlMixin.node_controller_spawner( - controller="joint_state_broadcaster" - ) - ld.add_action(joint_state_broadcaster) - robot_state_publisher = LBRROS2ControlMixin.node_robot_state_publisher( - robot_description=robot_description, use_sim_time=True - ) - ld.add_action( - robot_state_publisher - ) # Do not condition robot state publisher on joint state broadcaster as Gazebo uses robot state publisher to retrieve robot description + # launch arguments + ld.add_action(LBRDescriptionMixin.arg_model()) + ld.add_action(LBRDescriptionMixin.arg_robot_name()) ld.add_action( - LBRROS2ControlMixin.node_controller_spawner( - controller=LaunchConfiguration("ctrl") - ) - ) - - # MoveIt 2 - ld.add_action(LBRMoveGroupMixin.arg_allow_trajectory_execution()) - ld.add_action(LBRMoveGroupMixin.arg_capabilities()) - ld.add_action(LBRMoveGroupMixin.arg_disable_capabilities()) - ld.add_action(LBRMoveGroupMixin.arg_monitor_dynamics()) - ld.add_action(LBRMoveGroupMixin.args_publish_monitored_planning_scene()) + LBRROS2ControlMixin.arg_ctrl() + ) # Gazebo loads controller configuration through lbr_description/gazebo/*.xacro from lbr_ros2_control/config/lbr_controllers.yaml - # MoveGroup: - # - requires world frame - # - urdf only has robot_name/world - # This transform needs publishing - robot_name = LaunchConfiguration("robot_name").perform(context) + # static transform world -> robot_name/world + world_robot_tf = [0, 0, 0, 0, 0, 0] # keep zero ld.add_action( LBRDescriptionMixin.node_static_tf( tf=world_robot_tf, parent="world", - child=PathJoinSubstitution( - [ - robot_name, - "world", - ] # results in robot_name/world - ), - parameters=[{"use_sim_time": True}], + child=PathJoinSubstitution([LaunchConfiguration("robot_name"), "world"]), ) ) - model = LaunchConfiguration("model").perform(context) - moveit_configs_builder = LBRMoveGroupMixin.moveit_configs_builder( - robot_name=model, - package_name=f"{model}_moveit_config", - ) - move_group_params = LBRMoveGroupMixin.params_move_group() + # robot description + robot_description = LBRDescriptionMixin.param_robot_description(mode="gazebo") - ld.add_action( - LBRMoveGroupMixin.node_move_group( - parameters=[ - moveit_configs_builder.to_dict(), - move_group_params, - {"use_sim_time": True}, - ], - condition=IfCondition(LaunchConfiguration("moveit")), - namespace=robot_name, - ) - ) - - # RViz and MoveIt - rviz_moveit = RVizMixin.node_rviz( - rviz_config_pkg=f"{model}_moveit_config", - rviz_config="config/moveit.rviz", - parameters=LBRMoveGroupMixin.params_rviz( - moveit_configs=moveit_configs_builder.to_moveit_configs() - ) - + [{"use_sim_time": True}], - condition=IfCondition( - AndSubstitution(LaunchConfiguration("moveit"), LaunchConfiguration("rviz")) - ), - remappings=[ - ("display_planned_path", robot_name + "/display_planned_path"), - ("joint_states", robot_name + "/joint_states"), - ("monitored_planning_scene", robot_name + "/monitored_planning_scene"), - ("planning_scene", robot_name + "/planning_scene"), - ("planning_scene_world", robot_name + "/planning_scene_world"), - ("robot_description", robot_name + "/robot_description"), - ("robot_description_semantic", robot_name + "/robot_description_semantic"), - ], - ) - - # RViz no MoveIt - rviz = RVizMixin.node_rviz( - rviz_config_pkg="lbr_bringup", - rviz_config="config/config.rviz", - condition=IfCondition( - AndSubstitution( - LaunchConfiguration("rviz"), - NotSubstitution(LaunchConfiguration("moveit")), - ) - ), - ) - - # RViz event handler - rviz_event_handler = RegisterEventHandler( - OnProcessExit(target_action=spawn_entity, on_exit=[rviz_moveit, rviz]) + # robot state publisher + robot_state_publisher = LBRROS2ControlMixin.node_robot_state_publisher( + robot_description=robot_description, use_sim_time=True ) - ld.add_action(rviz_event_handler) - - return ld.entities + ld.add_action( + robot_state_publisher + ) # Do not condition robot state publisher on joint state broadcaster as Gazebo uses robot state publisher to retrieve robot description + # Gazebo + ld.add_action(GazeboMixin.include_gazebo()) # Gazebo has its own controller manager + spawn_entity = GazeboMixin.node_spawn_entity(tf=world_robot_tf) + ld.add_action(spawn_entity) -def generate_launch_description() -> LaunchDescription: - ld = LaunchDescription() - ld.add_action(LBRDescriptionMixin.arg_model()) - ld.add_action(LBRDescriptionMixin.arg_robot_name()) - ld.add_action( - DeclareLaunchArgument( - name="moveit", - default_value="false", - description="Whether to launch MoveIt 2.", - ) + # controllers + joint_state_broadcaster = LBRROS2ControlMixin.node_controller_spawner( + controller="joint_state_broadcaster" ) + ld.add_action(joint_state_broadcaster) ld.add_action( - DeclareLaunchArgument( - name="rviz", default_value="true", description="Whether to launch RViz." + LBRROS2ControlMixin.node_controller_spawner( + controller=LaunchConfiguration("ctrl") ) ) - ld.add_action( - LBRROS2ControlMixin.arg_ctrl() - ) # Gazebo loads controller configuration through lbr_description/gazebo/*.xacro from lbr_ros2_control/config/lbr_controllers.yaml - ld.add_action(OpaqueFunction(function=launch_setup)) return ld diff --git a/lbr_bringup/launch/hardware.launch.py b/lbr_bringup/launch/hardware.launch.py new file mode 100644 index 00000000..5240557f --- /dev/null +++ b/lbr_bringup/launch/hardware.launch.py @@ -0,0 +1,68 @@ +from launch import LaunchDescription +from launch.actions import RegisterEventHandler +from launch.event_handlers import OnProcessStart +from launch.substitutions import LaunchConfiguration, PathJoinSubstitution +from lbr_bringup.description import LBRDescriptionMixin +from lbr_bringup.ros2_control import LBRROS2ControlMixin + + +def generate_launch_description() -> LaunchDescription: + ld = LaunchDescription() + + # launch arguments + ld.add_action(LBRDescriptionMixin.arg_model()) + ld.add_action(LBRDescriptionMixin.arg_robot_name()) + ld.add_action(LBRDescriptionMixin.arg_port_id()) + ld.add_action(LBRROS2ControlMixin.arg_ctrl_cfg_pkg()) + ld.add_action(LBRROS2ControlMixin.arg_ctrl_cfg()) + ld.add_action(LBRROS2ControlMixin.arg_ctrl()) + + # static transform world -> robot_name/world + ld.add_action( + LBRDescriptionMixin.node_static_tf( + tf=[0.0, 0.0, 0.0, 0.0, 0.0, 0.0], + parent="world", + child=PathJoinSubstitution([LaunchConfiguration("robot_name"), "world"]), + ) + ) + + # robot description + robot_description = LBRDescriptionMixin.param_robot_description(mode="hardware") + + # robot state publisher + robot_state_publisher = LBRROS2ControlMixin.node_robot_state_publisher( + robot_description=robot_description, use_sim_time=False + ) + ld.add_action(robot_state_publisher) + + # ros2 control node + ros2_control_node = LBRROS2ControlMixin.node_ros2_control(use_sim_time=False) + ld.add_action(ros2_control_node) + + # joint state broad caster and controller on ros2 control node start + joint_state_broadcaster = LBRROS2ControlMixin.node_controller_spawner( + controller="joint_state_broadcaster" + ) + force_torque_broadcaster = LBRROS2ControlMixin.node_controller_spawner( + controller="force_torque_broadcaster" + ) + lbr_state_broadcaster = LBRROS2ControlMixin.node_controller_spawner( + controller="lbr_state_broadcaster" + ) + controller = LBRROS2ControlMixin.node_controller_spawner( + controller=LaunchConfiguration("ctrl") + ) + + controller_event_handler = RegisterEventHandler( + OnProcessStart( + target_action=ros2_control_node, + on_start=[ + joint_state_broadcaster, + force_torque_broadcaster, + lbr_state_broadcaster, + controller, + ], + ) + ) + ld.add_action(controller_event_handler) + return ld diff --git a/lbr_bringup/launch/mock.launch.py b/lbr_bringup/launch/mock.launch.py new file mode 100644 index 00000000..3443431f --- /dev/null +++ b/lbr_bringup/launch/mock.launch.py @@ -0,0 +1,59 @@ +from launch import LaunchDescription +from launch.actions import RegisterEventHandler +from launch.event_handlers import OnProcessStart +from launch.substitutions import LaunchConfiguration, PathJoinSubstitution +from lbr_bringup.description import LBRDescriptionMixin +from lbr_bringup.ros2_control import LBRROS2ControlMixin + + +def generate_launch_description() -> LaunchDescription: + ld = LaunchDescription() + + # launch arguments + ld.add_action(LBRDescriptionMixin.arg_model()) + ld.add_action(LBRDescriptionMixin.arg_robot_name()) + ld.add_action(LBRROS2ControlMixin.arg_ctrl_cfg_pkg()) + ld.add_action(LBRROS2ControlMixin.arg_ctrl_cfg()) + ld.add_action(LBRROS2ControlMixin.arg_ctrl()) + + # static transform world -> robot_name/world + ld.add_action( + LBRDescriptionMixin.node_static_tf( + tf=[0.0, 0.0, 0.0, 0.0, 0.0, 0.0], + parent="world", + child=PathJoinSubstitution([LaunchConfiguration("robot_name"), "world"]), + ) + ) + + # robot description + robot_description = LBRDescriptionMixin.param_robot_description(mode="mock") + + # robot state publisher + robot_state_publisher = LBRROS2ControlMixin.node_robot_state_publisher( + robot_description=robot_description, use_sim_time=False + ) + ld.add_action(robot_state_publisher) + + # ros2 control node + ros2_control_node = LBRROS2ControlMixin.node_ros2_control(use_sim_time=False) + ld.add_action(ros2_control_node) + + # joint state broad caster and controller on ros2 control node start + joint_state_broadcaster = LBRROS2ControlMixin.node_controller_spawner( + controller="joint_state_broadcaster" + ) + controller = LBRROS2ControlMixin.node_controller_spawner( + controller=LaunchConfiguration("ctrl") + ) + + controller_event_handler = RegisterEventHandler( + OnProcessStart( + target_action=ros2_control_node, + on_start=[ + joint_state_broadcaster, + controller, + ], + ) + ) + ld.add_action(controller_event_handler) + return ld diff --git a/lbr_bringup/launch/move_group.launch.py b/lbr_bringup/launch/move_group.launch.py index 29f68bb3..1fc8c06a 100644 --- a/lbr_bringup/launch/move_group.launch.py +++ b/lbr_bringup/launch/move_group.launch.py @@ -2,9 +2,9 @@ from launch import LaunchContext, LaunchDescription, LaunchDescriptionEntity from launch.actions import OpaqueFunction -from launch.substitutions import LaunchConfiguration, PathJoinSubstitution -from launch_mixins.lbr_bringup import LBRMoveGroupMixin -from launch_mixins.lbr_description import LBRDescriptionMixin, RVizMixin +from launch.substitutions import LaunchConfiguration +from lbr_bringup.description import LBRDescriptionMixin, RVizMixin +from lbr_bringup.move_group import LBRMoveGroupMixin def launch_setup(context: LaunchContext) -> List[LaunchDescriptionEntity]: @@ -23,30 +23,19 @@ def launch_setup(context: LaunchContext) -> List[LaunchDescriptionEntity]: ) move_group_params = LBRMoveGroupMixin.params_move_group() - # MoveGroup: - # - requires world frame - # - urdf only has robot_name/world - # This transform needs publishing - robot_name = LaunchConfiguration("robot_name").perform(context) - ld.add_action( - LBRDescriptionMixin.node_static_tf( - tf=[0, 0, 0, 0, 0, 0], # keep zero - parent="world", - child=PathJoinSubstitution( - [ - robot_name, - "world", - ] # results in robot_name/world - ), - ) - ) + mode = LaunchConfiguration("mode").perform(context) + use_sim_time = False + if mode == "gazebo": + use_sim_time = True + # MoveGroup + robot_name = LaunchConfiguration("robot_name").perform(context) ld.add_action( LBRMoveGroupMixin.node_move_group( parameters=[ moveit_configs_builder.to_dict(), move_group_params, - {"use_sim_time": LaunchConfiguration("sim")}, + {"use_sim_time": use_sim_time}, ], namespace=robot_name, ) @@ -59,7 +48,7 @@ def launch_setup(context: LaunchContext) -> List[LaunchDescriptionEntity]: parameters=LBRMoveGroupMixin.params_rviz( moveit_configs=moveit_configs_builder.to_moveit_configs() ) - + [{"use_sim_time": LaunchConfiguration("sim")}], + + [{"use_sim_time": use_sim_time}], remappings=[ ("display_planned_path", robot_name + "/display_planned_path"), ("joint_states", robot_name + "/joint_states"), @@ -79,9 +68,10 @@ def launch_setup(context: LaunchContext) -> List[LaunchDescriptionEntity]: def generate_launch_description() -> LaunchDescription: ld = LaunchDescription() + ld.add_action(LBRDescriptionMixin.arg_mode()) ld.add_action(LBRDescriptionMixin.arg_model()) ld.add_action(LBRDescriptionMixin.arg_robot_name()) - ld.add_action(LBRDescriptionMixin.arg_sim()) + ld.add_action(LBRDescriptionMixin.arg_mode()) ld.add_action(OpaqueFunction(function=launch_setup)) diff --git a/lbr_bringup/launch/real.launch.py b/lbr_bringup/launch/real.launch.py deleted file mode 100644 index bc7f0c04..00000000 --- a/lbr_bringup/launch/real.launch.py +++ /dev/null @@ -1,171 +0,0 @@ -from typing import List - -from launch import LaunchContext, LaunchDescription, LaunchDescriptionEntity -from launch.actions import DeclareLaunchArgument, OpaqueFunction, RegisterEventHandler -from launch.conditions import IfCondition -from launch.event_handlers import OnProcessStart -from launch.substitutions import ( - AndSubstitution, - LaunchConfiguration, - NotSubstitution, - PathJoinSubstitution, -) -from launch_mixins.lbr_bringup import LBRMoveGroupMixin -from launch_mixins.lbr_description import LBRDescriptionMixin, RVizMixin -from launch_mixins.lbr_ros2_control import LBRROS2ControlMixin - - -def launch_setup(context: LaunchContext) -> List[LaunchDescriptionEntity]: - ld = LaunchDescription() - - robot_description = LBRDescriptionMixin.param_robot_description(sim=False) - world_robot_tf = [0, 0, 0, 0, 0, 0] # keep zero - - # robot state publisher - robot_state_publisher = LBRROS2ControlMixin.node_robot_state_publisher( - robot_description=robot_description, use_sim_time=False - ) - ld.add_action(robot_state_publisher) - - # ros2 control node - ros2_control_node = LBRROS2ControlMixin.node_ros2_control() - ld.add_action(ros2_control_node) - - # joint state broad caster and controller on ros2 control node start - joint_state_broadcaster = LBRROS2ControlMixin.node_controller_spawner( - controller="joint_state_broadcaster" - ) - force_torque_broadcaster = LBRROS2ControlMixin.node_controller_spawner( - controller="force_torque_broadcaster" - ) - lbr_state_broadcaster = LBRROS2ControlMixin.node_controller_spawner( - controller="lbr_state_broadcaster" - ) - controller = LBRROS2ControlMixin.node_controller_spawner( - controller=LaunchConfiguration("ctrl") - ) - - controller_event_handler = RegisterEventHandler( - OnProcessStart( - target_action=ros2_control_node, - on_start=[ - joint_state_broadcaster, - force_torque_broadcaster, - lbr_state_broadcaster, - controller, - ], - ) - ) - ld.add_action(controller_event_handler) - - # MoveIt 2 - ld.add_action(LBRMoveGroupMixin.arg_allow_trajectory_execution()) - ld.add_action(LBRMoveGroupMixin.arg_capabilities()) - ld.add_action(LBRMoveGroupMixin.arg_disable_capabilities()) - ld.add_action(LBRMoveGroupMixin.arg_monitor_dynamics()) - ld.add_action(LBRMoveGroupMixin.args_publish_monitored_planning_scene()) - - # MoveGroup: - # - requires world frame - # - urdf only has robot_name/world - # This transform needs publishing - robot_name = LaunchConfiguration("robot_name").perform(context) - ld.add_action( - LBRDescriptionMixin.node_static_tf( - tf=world_robot_tf, - parent="world", - child=PathJoinSubstitution( - [ - robot_name, - "world", - ] # results in robot_name/world - ), - ) - ) - - model = LaunchConfiguration("model").perform(context) - moveit_configs_builder = LBRMoveGroupMixin.moveit_configs_builder( - robot_name=model, - package_name=f"{model}_moveit_config", - ) - move_group_params = LBRMoveGroupMixin.params_move_group() - - ld.add_action( - LBRMoveGroupMixin.node_move_group( - parameters=[ - moveit_configs_builder.to_dict(), - move_group_params, - {"use_sim_time": False}, - ], - condition=IfCondition(LaunchConfiguration("moveit")), - namespace=robot_name, - ) - ) - - # RViz and MoveIt - rviz_moveit = RVizMixin.node_rviz( - rviz_config_pkg=f"{model}_moveit_config", - rviz_config="config/moveit.rviz", - parameters=LBRMoveGroupMixin.params_rviz( - moveit_configs=moveit_configs_builder.to_moveit_configs() - ) - + [{"use_sim_time": False}], - condition=IfCondition( - AndSubstitution(LaunchConfiguration("moveit"), LaunchConfiguration("rviz")) - ), - remappings=[ - ("display_planned_path", robot_name + "/display_planned_path"), - ("joint_states", robot_name + "/joint_states"), - ("monitored_planning_scene", robot_name + "/monitored_planning_scene"), - ("planning_scene", robot_name + "/planning_scene"), - ("planning_scene_world", robot_name + "/planning_scene_world"), - ("robot_description", robot_name + "/robot_description"), - ("robot_description_semantic", robot_name + "/robot_description_semantic"), - ], - ) - - # RViz no MoveIt - rviz = RVizMixin.node_rviz( - rviz_config_pkg="lbr_bringup", - rviz_config="config/config.rviz", - condition=IfCondition( - AndSubstitution( - LaunchConfiguration("rviz"), - NotSubstitution(LaunchConfiguration("moveit")), - ) - ), - ) - - # RViz event handler - rviz_event_handler = RegisterEventHandler( - OnProcessStart( - target_action=joint_state_broadcaster, on_start=[rviz_moveit, rviz] - ) - ) - ld.add_action(rviz_event_handler) - - return ld.entities - - -def generate_launch_description() -> LaunchDescription: - ld = LaunchDescription() - ld.add_action(LBRDescriptionMixin.arg_model()) - ld.add_action(LBRDescriptionMixin.arg_robot_name()) - ld.add_action(LBRDescriptionMixin.arg_port_id()) - ld.add_action( - DeclareLaunchArgument( - name="moveit", - default_value="false", - description="Whether to launch MoveIt 2.", - ) - ) - ld.add_action( - DeclareLaunchArgument( - name="rviz", default_value="true", description="Whether to launch RViz." - ) - ) - ld.add_action(LBRROS2ControlMixin.arg_ctrl_cfg_pkg()) - ld.add_action(LBRROS2ControlMixin.arg_ctrl_cfg()) - ld.add_action(LBRROS2ControlMixin.arg_ctrl()) - ld.add_action(OpaqueFunction(function=launch_setup)) - return ld diff --git a/lbr_bringup/launch/fake_hardware.launch.py b/lbr_bringup/launch/rviz2.launch.py similarity index 100% rename from lbr_bringup/launch/fake_hardware.launch.py rename to lbr_bringup/launch/rviz2.launch.py diff --git a/lbr_bringup/launch/system_interface.launch.py b/lbr_bringup/launch/system_interface.launch.py deleted file mode 100644 index efc53910..00000000 --- a/lbr_bringup/launch/system_interface.launch.py +++ /dev/null @@ -1,53 +0,0 @@ -from launch import LaunchDescription -from launch.actions import RegisterEventHandler -from launch.event_handlers import OnProcessStart -from launch.substitutions import LaunchConfiguration -from launch_mixins.lbr_description import LBRDescriptionMixin -from launch_mixins.lbr_ros2_control import LBRROS2ControlMixin - - -class LBRSystemInterface(LBRDescriptionMixin, LBRROS2ControlMixin): - pass - - -def generate_launch_description() -> LaunchDescription: - ld = LaunchDescription() - ld.add_action(LBRSystemInterface.arg_model()) - ld.add_action(LBRSystemInterface.arg_robot_name()) - ld.add_action(LBRSystemInterface.arg_port_id()) - robot_description = LBRSystemInterface.param_robot_description(sim=False) - ld.add_action(LBRSystemInterface.arg_ctrl_cfg_pkg()) - ld.add_action(LBRSystemInterface.arg_ctrl_cfg()) - ld.add_action(LBRSystemInterface.arg_ctrl()) - robot_state_publisher = LBRSystemInterface.node_robot_state_publisher( - robot_description=robot_description, - use_sim_time=False, - ) - ld.add_action(robot_state_publisher) - ros2_control_node = LBRSystemInterface.node_ros2_control() - ld.add_action(ros2_control_node) - joint_state_broadcaster = LBRSystemInterface.node_controller_spawner( - controller="joint_state_broadcaster" - ) - force_torque_broadcaster = LBRSystemInterface.node_controller_spawner( - controller="force_torque_broadcaster" - ) - lbr_state_broadcaster = LBRSystemInterface.node_controller_spawner( - controller="lbr_state_broadcaster" - ) - controller = LBRSystemInterface.node_controller_spawner( - controller=LaunchConfiguration("ctrl") - ) - controller_event_handler = RegisterEventHandler( - OnProcessStart( - target_action=ros2_control_node, - on_start=[ - joint_state_broadcaster, - force_torque_broadcaster, - lbr_state_broadcaster, - controller, - ], - ) - ) - ld.add_action(controller_event_handler) - return ld diff --git a/lbr_bringup/lbr_bringup/lbr_description.py b/lbr_bringup/lbr_bringup/description.py similarity index 91% rename from lbr_bringup/lbr_bringup/lbr_description.py rename to lbr_bringup/lbr_bringup/description.py index c91f2f7e..797a8c86 100644 --- a/lbr_bringup/lbr_bringup/lbr_description.py +++ b/lbr_bringup/lbr_bringup/description.py @@ -66,12 +66,10 @@ def param_robot_description( port_id: Optional[Union[LaunchConfiguration, str]] = LaunchConfiguration( "port_id", default="30200" ), - sim: Optional[Union[LaunchConfiguration, bool]] = LaunchConfiguration( - "sim", default="true" + mode: Optional[Union[LaunchConfiguration, bool]] = LaunchConfiguration( + "mode", default="mock" ), ) -> Dict[str, str]: - if type(sim) is bool: - sim = "true" if sim else "false" robot_description = { "robot_description": Command( [ @@ -90,8 +88,8 @@ def param_robot_description( robot_name, " port_id:=", port_id, - " sim:=", - sim, + " mode:=", + mode, ] ) } @@ -124,11 +122,16 @@ def arg_port_id(default_value: str = "30200") -> DeclareLaunchArgument: ) @staticmethod - def arg_sim(default_value: str = "true") -> DeclareLaunchArgument: + def arg_mode(default_value: str = "mock") -> DeclareLaunchArgument: return DeclareLaunchArgument( - name="sim", + name="mode", default_value=default_value, - description="Whether to use the simulation or not.", + description="The mode to launch in.", + choices=[ + "mock", + "hardware", + "gazebo", + ], ) @staticmethod @@ -140,8 +143,8 @@ def param_port_id() -> Dict[str, LaunchConfiguration]: return {"port_id": LaunchConfiguration("port_id", default="30200")} @staticmethod - def param_sim() -> Dict[str, LaunchConfiguration]: - return {"sim": LaunchConfiguration("sim", default="true")} + def param_mode() -> Dict[str, LaunchConfiguration]: + return {"mode": LaunchConfiguration("mode", default="mock")} @staticmethod def node_static_tf( diff --git a/lbr_bringup/lbr_bringup/lbr_bringup.py b/lbr_bringup/lbr_bringup/move_group.py similarity index 100% rename from lbr_bringup/lbr_bringup/lbr_bringup.py rename to lbr_bringup/lbr_bringup/move_group.py diff --git a/lbr_bringup/lbr_bringup/lbr_ros2_control.py b/lbr_bringup/lbr_bringup/ros2_control.py similarity index 95% rename from lbr_bringup/lbr_bringup/lbr_ros2_control.py rename to lbr_bringup/lbr_bringup/ros2_control.py index fd270768..a5a58627 100644 --- a/lbr_bringup/lbr_bringup/lbr_ros2_control.py +++ b/lbr_bringup/lbr_bringup/ros2_control.py @@ -51,13 +51,16 @@ def node_ros2_control( robot_name: Optional[Union[LaunchConfiguration, str]] = LaunchConfiguration( "robot_name", default="lbr" ), + use_sim_time: Optional[Union[LaunchConfiguration, bool]] = LaunchConfiguration( + "use_sim_time", default="false" + ), **kwargs, ) -> Node: return Node( package="controller_manager", executable="ros2_control_node", parameters=[ - {"use_sim_time": False}, + {"use_sim_time": use_sim_time}, PathJoinSubstitution( [ FindPackageShare( diff --git a/lbr_description/urdf/iiwa14/iiwa14.xacro b/lbr_description/urdf/iiwa14/iiwa14.xacro index e936c2a6..7746f30b 100644 --- a/lbr_description/urdf/iiwa14/iiwa14.xacro +++ b/lbr_description/urdf/iiwa14/iiwa14.xacro @@ -7,7 +7,7 @@ - + @@ -25,6 +25,6 @@ \ No newline at end of file diff --git a/lbr_description/urdf/iiwa14/iiwa14_description.xacro b/lbr_description/urdf/iiwa14/iiwa14_description.xacro index 7e63d77f..712fc099 100644 --- a/lbr_description/urdf/iiwa14/iiwa14_description.xacro +++ b/lbr_description/urdf/iiwa14/iiwa14_description.xacro @@ -3,7 +3,7 @@ + params="robot_name:=^|lbr mode:=^|mock system_parameters_path:=^|'$(find lbr_ros2_control)/config/lbr_system_parameters.yaml'"> @@ -19,10 +19,10 @@ - + diff --git a/lbr_description/urdf/iiwa7/iiwa7.xacro b/lbr_description/urdf/iiwa7/iiwa7.xacro index a451cf70..501160d4 100644 --- a/lbr_description/urdf/iiwa7/iiwa7.xacro +++ b/lbr_description/urdf/iiwa7/iiwa7.xacro @@ -7,7 +7,7 @@ - + @@ -25,6 +25,6 @@ \ No newline at end of file diff --git a/lbr_description/urdf/iiwa7/iiwa7_description.xacro b/lbr_description/urdf/iiwa7/iiwa7_description.xacro index 6f4688c4..d177bb80 100644 --- a/lbr_description/urdf/iiwa7/iiwa7_description.xacro +++ b/lbr_description/urdf/iiwa7/iiwa7_description.xacro @@ -3,7 +3,7 @@ + params="robot_name:=^|lbr mode:=^|mock system_parameters_path:=^|'$(find lbr_ros2_control)/config/lbr_system_parameters.yaml'"> @@ -19,10 +19,10 @@ - + diff --git a/lbr_description/urdf/med14/med14.xacro b/lbr_description/urdf/med14/med14.xacro index c3abc0e9..89881aaa 100644 --- a/lbr_description/urdf/med14/med14.xacro +++ b/lbr_description/urdf/med14/med14.xacro @@ -7,7 +7,7 @@ - + @@ -25,6 +25,6 @@ \ No newline at end of file diff --git a/lbr_description/urdf/med14/med14_description.xacro b/lbr_description/urdf/med14/med14_description.xacro index e0fc40e2..0eac46fe 100644 --- a/lbr_description/urdf/med14/med14_description.xacro +++ b/lbr_description/urdf/med14/med14_description.xacro @@ -3,7 +3,7 @@ + params="robot_name:=^|lbr mode:=^|mock system_parameters_path:=^|'$(find lbr_ros2_control)/config/lbr_system_parameters.yaml'"> @@ -19,10 +19,10 @@ - + diff --git a/lbr_description/urdf/med7/med7.xacro b/lbr_description/urdf/med7/med7.xacro index 0e2fe380..e34caeb8 100644 --- a/lbr_description/urdf/med7/med7.xacro +++ b/lbr_description/urdf/med7/med7.xacro @@ -7,7 +7,7 @@ - + @@ -25,6 +25,6 @@ \ No newline at end of file diff --git a/lbr_description/urdf/med7/med7_description.xacro b/lbr_description/urdf/med7/med7_description.xacro index 43ef52df..58c885c4 100644 --- a/lbr_description/urdf/med7/med7_description.xacro +++ b/lbr_description/urdf/med7/med7_description.xacro @@ -3,7 +3,7 @@ + params="robot_name:=^|lbr mode:=^|mock system_parameters_path:=^|'$(find lbr_ros2_control)/config/lbr_system_parameters.yaml'"> @@ -19,10 +19,10 @@ - + diff --git a/lbr_ros2_control/config/lbr_system_interface.xacro b/lbr_ros2_control/config/lbr_system_interface.xacro index 3e9ec6cc..ab2e8664 100644 --- a/lbr_ros2_control/config/lbr_system_interface.xacro +++ b/lbr_ros2_control/config/lbr_system_interface.xacro @@ -1,19 +1,24 @@ + params="mode joint_limits system_parameters_path"> - - + + + + mock_components/GenericSystem + + + gazebo_ros2_control/GazeboSystem - + lbr_ros2_control::SystemInterface ${system_parameters['hardware']['fri_client_sdk']['major_version']} @@ -33,11 +38,11 @@ ${system_parameters['hardware']['measured_torque_cutoff_frequency']} ${system_parameters['hardware']['open_loop']} - + - + @@ -83,16 +88,12 @@ - + + params="name min_position max_position max_velocity max_torque mode"> - ${min_position} - ${max_position} - ${max_velocity} - ${max_torque} ${min_position} ${max_position} @@ -104,14 +105,18 @@ - + + ${min_position} + ${max_position} + ${max_velocity} + ${max_torque} - + @@ -120,43 +125,43 @@ max_position="${joint_limits['A1']['upper'] * PI / 180}" max_velocity="${joint_limits['A1']['velocity'] * PI / 180}" max_torque="${joint_limits['A1']['effort']}" - sim="${sim}" /> + mode="${mode}" /> + mode="${mode}" /> + mode="${mode}" /> + mode="${mode}" /> + mode="${mode}" /> + mode="${mode}" /> + mode="${mode}" /> \ No newline at end of file From 64b1781075c76be35871580944a82db38bb85855 Mon Sep 17 00:00:00 2001 From: mhubii Date: Tue, 10 Sep 2024 11:58:20 +0200 Subject: [PATCH 04/15] launch de-coupling --- CHANGELOG.rst | 13 ++ .../config/gazebo.rviz | 63 +++--- .../config/{config.rviz => hardware.rviz} | 0 lbr_bringup/config/mock.rviz | 209 ++++++++++++++++++ lbr_bringup/launch/gazebo.launch.py | 7 +- lbr_bringup/launch/move_group.launch.py | 24 +- lbr_bringup/launch/rviz.launch.py | 32 +++ lbr_bringup/launch/rviz2.launch.py | 0 lbr_bringup/lbr_bringup/description.py | 93 +------- lbr_bringup/lbr_bringup/gazebo.py | 49 ++++ lbr_bringup/lbr_bringup/rviz.py | 54 +++++ .../lbr_moveit_cpp/doc/lbr_moveit_cpp.rst | 2 +- .../launch/hello_moveit.launch.py | 12 +- 13 files changed, 405 insertions(+), 153 deletions(-) rename lbr_description/config/config.rviz => lbr_bringup/config/gazebo.rviz (77%) rename lbr_bringup/config/{config.rviz => hardware.rviz} (100%) create mode 100644 lbr_bringup/config/mock.rviz create mode 100644 lbr_bringup/launch/rviz.launch.py delete mode 100644 lbr_bringup/launch/rviz2.launch.py create mode 100644 lbr_bringup/lbr_bringup/gazebo.py create mode 100644 lbr_bringup/lbr_bringup/rviz.py diff --git a/CHANGELOG.rst b/CHANGELOG.rst index 6fccbd26..75448fe2 100644 --- a/CHANGELOG.rst +++ b/CHANGELOG.rst @@ -1,6 +1,19 @@ ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ Changelog for package LBR FRI ROS 2 Stack ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Humble v2.1.0 (2024-09-10) +-------------------------- +* De-couple launch files from ``lbr_bringup`` for easier customization (breaking change): + + * Removed ``sim:=true / false`` argument from launch files in favor of dedicated launch files (since no feature parity between simulation and real robot) + * MoveIt and RViz need to be launched separately now + * User can now launch via: + + * ``ros2 launch lbr_bringup mock.launch.py`` (new: mock system) + * ``ros2 launch lbr_bringup hardware.launch.py`` (real robot) + * ``ros2 launch lbr_bringup gazebo.launch.py`` (Gazebo simulation) +* Added mock hardware to ``lbr_ros2_control`` (for simple ``ros2_control`` testing without the need for Gazebo) + Humble v2.0.0 (2024-07-08) -------------------------- * Remove ``app.launch.py`` from demos in favor for ``ros2_control`` variant (breaking change) diff --git a/lbr_description/config/config.rviz b/lbr_bringup/config/gazebo.rviz similarity index 77% rename from lbr_description/config/config.rviz rename to lbr_bringup/config/gazebo.rviz index f0e17afd..f4486f40 100644 --- a/lbr_description/config/config.rviz +++ b/lbr_bringup/config/gazebo.rviz @@ -1,14 +1,15 @@ Panels: - Class: rviz_common/Displays - Help Height: 78 + Help Height: 87 Name: Displays Property Tree Widget: Expanded: - /Global Options1 - /Status1 - /RobotModel1 - Splitter Ratio: 0.5 - Tree Height: 787 + - /RobotModel1/Description Topic1 + Splitter Ratio: 0.40760868787765503 + Tree Height: 657 - Class: rviz_common/Selection Name: Selection - Class: rviz_common/Tool Properties @@ -22,6 +23,11 @@ Panels: - /Current View1 Name: Views Splitter Ratio: 0.5 + - Class: rviz_common/Time + Experimental: false + Name: Time + SyncMode: 0 + SyncSource: "" Visualization Manager: Class: "" Displays: @@ -53,7 +59,7 @@ Visualization Manager: Durability Policy: Volatile History Policy: Keep Last Reliability Policy: Reliable - Value: /robot_description + Value: /lbr/robot_description Enabled: true Links: All Links Enabled: true @@ -105,25 +111,15 @@ Visualization Manager: Alpha: 1 Show Axes: false Show Trail: false - storz_straight_exoscope_hsi_link_0: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - storz_straight_exoscope_hsi_link_cm: - Alpha: 1 - Show Axes: false - Show Trail: false - storz_straight_exoscope_hsi_link_cm_optical: - Alpha: 1 - Show Axes: false - Show Trail: false world: Alpha: 1 Show Axes: false Show Trail: false + Mass Properties: + Inertia: false + Mass: false Name: RobotModel - TF Prefix: "" + TF Prefix: lbr Update Interval: 0 Value: true Visual Enabled: true @@ -142,6 +138,9 @@ Visualization Manager: - Class: rviz_default_plugins/Measure Line color: 128; 128; 0 - Class: rviz_default_plugins/SetInitialPose + Covariance x: 0.25 + Covariance y: 0.25 + Covariance yaw: 0.06853891909122467 Topic: Depth: 5 Durability Policy: Volatile @@ -170,39 +169,41 @@ Visualization Manager: Views: Current: Class: rviz_default_plugins/Orbit - Distance: 4.795630931854248 + Distance: 3.38482666015625 Enable Stereo Rendering: Stereo Eye Separation: 0.05999999865889549 Stereo Focal Distance: 1 Swap Stereo Eyes: false Value: false Focal Point: - X: -0.19523456692695618 - Y: -0.17667146027088165 - Z: 0.2629773020744324 - Focal Shape Fixed Size: true + X: -0.10796727985143661 + Y: -0.08450233191251755 + Z: 0.1360965371131897 + Focal Shape Fixed Size: false Focal Shape Size: 0.05000000074505806 Invert Z Axis: false Name: Current View Near Clip Distance: 0.009999999776482582 - Pitch: 0.785398006439209 + Pitch: 0.7853981852531433 Target Frame: - Value: Orbit (rviz_default_plugins) - Yaw: 0.785398006439209 + Value: Orbit (rviz) + Yaw: 0.7853981852531433 Saved: ~ Window Geometry: Displays: collapsed: false - Height: 1016 + Height: 1021 Hide Left Dock: false Hide Right Dock: false - QMainWindow State: 000000ff00000000fd0000000400000000000001ba0000039efc0200000008fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005c00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000003d0000039e000000c900fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261000000010000010f0000039efc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073010000003d0000039e000000a400fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000004420000003efc0100000002fb0000000800540069006d00650100000000000004420000000000000000fb0000000800540069006d00650100000000000004500000000000000000000004ab0000039e00000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 + QMainWindow State: 000000ff00000000fd00000004000000000000021200000337fc0200000008fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000007901000003fb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000004e00000337000000ff01000003fb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261000000010000010f0000035bfc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073000000003f0000035b000000d701000003fb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e100000197000000030000073a0000004efc0100000002fb0000000800540069006d006501000000000000073a000002e701000003fb0000000800540069006d00650100000000000004500000000000000000000005270000033700000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 Selection: collapsed: false + Time: + collapsed: false Tool Properties: collapsed: false Views: collapsed: false - Width: 1920 - X: 0 - Y: 27 + Width: 1850 + X: 1080 + Y: 450 diff --git a/lbr_bringup/config/config.rviz b/lbr_bringup/config/hardware.rviz similarity index 100% rename from lbr_bringup/config/config.rviz rename to lbr_bringup/config/hardware.rviz diff --git a/lbr_bringup/config/mock.rviz b/lbr_bringup/config/mock.rviz new file mode 100644 index 00000000..f4486f40 --- /dev/null +++ b/lbr_bringup/config/mock.rviz @@ -0,0 +1,209 @@ +Panels: + - Class: rviz_common/Displays + Help Height: 87 + Name: Displays + Property Tree Widget: + Expanded: + - /Global Options1 + - /Status1 + - /RobotModel1 + - /RobotModel1/Description Topic1 + Splitter Ratio: 0.40760868787765503 + Tree Height: 657 + - Class: rviz_common/Selection + Name: Selection + - Class: rviz_common/Tool Properties + Expanded: + - /2D Goal Pose1 + - /Publish Point1 + Name: Tool Properties + Splitter Ratio: 0.5886790156364441 + - Class: rviz_common/Views + Expanded: + - /Current View1 + Name: Views + Splitter Ratio: 0.5 + - Class: rviz_common/Time + Experimental: false + Name: Time + SyncMode: 0 + SyncSource: "" +Visualization Manager: + Class: "" + Displays: + - Alpha: 0.5 + Cell Size: 1 + Class: rviz_default_plugins/Grid + Color: 34; 141; 255 + Enabled: true + Line Style: + Line Width: 0.029999999329447746 + Value: Lines + Name: Grid + Normal Cell Count: 0 + Offset: + X: 0 + Y: 0 + Z: 0 + Plane: XY + Plane Cell Count: 10 + Reference Frame: + Value: true + - Alpha: 1 + Class: rviz_default_plugins/RobotModel + Collision Enabled: false + Description File: "" + Description Source: Topic + Description Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /lbr/robot_description + Enabled: true + Links: + All Links Enabled: true + Expand Joint Details: false + Expand Link Details: false + Expand Tree: false + Link Tree Style: Links in Alphabetic Order + link_0: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + link_1: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + link_2: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + link_3: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + link_4: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + link_5: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + link_6: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + link_7: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + link_ee: + Alpha: 1 + Show Axes: false + Show Trail: false + world: + Alpha: 1 + Show Axes: false + Show Trail: false + Mass Properties: + Inertia: false + Mass: false + Name: RobotModel + TF Prefix: lbr + Update Interval: 0 + Value: true + Visual Enabled: true + Enabled: true + Global Options: + Background Color: 21; 21; 26 + Fixed Frame: world + Frame Rate: 30 + Name: root + Tools: + - Class: rviz_default_plugins/Interact + Hide Inactive Objects: true + - Class: rviz_default_plugins/MoveCamera + - Class: rviz_default_plugins/Select + - Class: rviz_default_plugins/FocusCamera + - Class: rviz_default_plugins/Measure + Line color: 128; 128; 0 + - Class: rviz_default_plugins/SetInitialPose + Covariance x: 0.25 + Covariance y: 0.25 + Covariance yaw: 0.06853891909122467 + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /initialpose + - Class: rviz_default_plugins/SetGoal + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /goal_pose + - Class: rviz_default_plugins/PublishPoint + Single click: true + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /clicked_point + Transformation: + Current: + Class: rviz_default_plugins/TF + Value: true + Views: + Current: + Class: rviz_default_plugins/Orbit + Distance: 3.38482666015625 + Enable Stereo Rendering: + Stereo Eye Separation: 0.05999999865889549 + Stereo Focal Distance: 1 + Swap Stereo Eyes: false + Value: false + Focal Point: + X: -0.10796727985143661 + Y: -0.08450233191251755 + Z: 0.1360965371131897 + Focal Shape Fixed Size: false + Focal Shape Size: 0.05000000074505806 + Invert Z Axis: false + Name: Current View + Near Clip Distance: 0.009999999776482582 + Pitch: 0.7853981852531433 + Target Frame: + Value: Orbit (rviz) + Yaw: 0.7853981852531433 + Saved: ~ +Window Geometry: + Displays: + collapsed: false + Height: 1021 + Hide Left Dock: false + Hide Right Dock: false + QMainWindow State: 000000ff00000000fd00000004000000000000021200000337fc0200000008fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000007901000003fb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000004e00000337000000ff01000003fb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261000000010000010f0000035bfc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073000000003f0000035b000000d701000003fb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e100000197000000030000073a0000004efc0100000002fb0000000800540069006d006501000000000000073a000002e701000003fb0000000800540069006d00650100000000000004500000000000000000000005270000033700000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 + Selection: + collapsed: false + Time: + collapsed: false + Tool Properties: + collapsed: false + Views: + collapsed: false + Width: 1850 + X: 1080 + Y: 450 diff --git a/lbr_bringup/launch/gazebo.launch.py b/lbr_bringup/launch/gazebo.launch.py index ee76f9b3..0ff93e5b 100644 --- a/lbr_bringup/launch/gazebo.launch.py +++ b/lbr_bringup/launch/gazebo.launch.py @@ -1,6 +1,7 @@ from launch import LaunchDescription from launch.substitutions import LaunchConfiguration, PathJoinSubstitution -from lbr_bringup.description import GazeboMixin, LBRDescriptionMixin +from lbr_bringup.description import LBRDescriptionMixin +from lbr_bringup.gazebo import GazeboMixin from lbr_bringup.ros2_control import LBRROS2ControlMixin @@ -37,7 +38,9 @@ def generate_launch_description() -> LaunchDescription: # Gazebo ld.add_action(GazeboMixin.include_gazebo()) # Gazebo has its own controller manager - spawn_entity = GazeboMixin.node_spawn_entity(tf=world_robot_tf) + spawn_entity = GazeboMixin.node_spawn_entity( + tf=world_robot_tf + ) # spawns robot in Gazebo through robot_description topic of robot_state_publisher ld.add_action(spawn_entity) # controllers diff --git a/lbr_bringup/launch/move_group.launch.py b/lbr_bringup/launch/move_group.launch.py index 1fc8c06a..c9796c9a 100644 --- a/lbr_bringup/launch/move_group.launch.py +++ b/lbr_bringup/launch/move_group.launch.py @@ -3,7 +3,7 @@ from launch import LaunchContext, LaunchDescription, LaunchDescriptionEntity from launch.actions import OpaqueFunction from launch.substitutions import LaunchConfiguration -from lbr_bringup.description import LBRDescriptionMixin, RVizMixin +from lbr_bringup.description import LBRDescriptionMixin from lbr_bringup.move_group import LBRMoveGroupMixin @@ -40,28 +40,6 @@ def launch_setup(context: LaunchContext) -> List[LaunchDescriptionEntity]: namespace=robot_name, ) ) - - # RViz - rviz = RVizMixin.node_rviz( - rviz_config_pkg=f"{model}_moveit_config", - rviz_config="config/moveit.rviz", - parameters=LBRMoveGroupMixin.params_rviz( - moveit_configs=moveit_configs_builder.to_moveit_configs() - ) - + [{"use_sim_time": use_sim_time}], - remappings=[ - ("display_planned_path", robot_name + "/display_planned_path"), - ("joint_states", robot_name + "/joint_states"), - ("monitored_planning_scene", robot_name + "/monitored_planning_scene"), - ("planning_scene", robot_name + "/planning_scene"), - ("planning_scene_world", robot_name + "/planning_scene_world"), - ("robot_description", robot_name + "/robot_description"), - ("robot_description_semantic", robot_name + "/robot_description_semantic"), - ], - ) - - ld.add_action(rviz) - return ld.entities diff --git a/lbr_bringup/launch/rviz.launch.py b/lbr_bringup/launch/rviz.launch.py new file mode 100644 index 00000000..aed07293 --- /dev/null +++ b/lbr_bringup/launch/rviz.launch.py @@ -0,0 +1,32 @@ +from typing import List + +from launch import LaunchContext, LaunchDescription, LaunchDescriptionEntity +from launch.actions import OpaqueFunction +from launch.substitutions import LaunchConfiguration +from lbr_bringup.description import LBRDescriptionMixin +from lbr_bringup.rviz import RVizMixin + + +def launch_setup(context: LaunchContext) -> List[LaunchDescriptionEntity]: + ld = LaunchDescription() + mode = LaunchConfiguration("mode").perform(context) + + # nodes + ld.add_action( + RVizMixin.node_rviz( + rviz_config_pkg="lbr_bringup", + rviz_config=f"config/{mode}.rviz", + ) + ) + return ld.entities + + +def generate_launch_description() -> LaunchDescription: + ld = LaunchDescription() + + # launch arguments + ld.add_action(LBRDescriptionMixin.arg_mode()) + + # rviz with mode-specific configuration + ld.add_action(OpaqueFunction(function=launch_setup)) + return ld diff --git a/lbr_bringup/launch/rviz2.launch.py b/lbr_bringup/launch/rviz2.launch.py deleted file mode 100644 index e69de29b..00000000 diff --git a/lbr_bringup/lbr_bringup/description.py b/lbr_bringup/lbr_bringup/description.py index 797a8c86..5981b84f 100644 --- a/lbr_bringup/lbr_bringup/description.py +++ b/lbr_bringup/lbr_bringup/description.py @@ -1,7 +1,6 @@ from typing import Dict, List, Optional, Union -from launch.actions import DeclareLaunchArgument, IncludeLaunchDescription -from launch.launch_description_sources import PythonLaunchDescriptionSource +from launch.actions import DeclareLaunchArgument from launch.substitutions import ( Command, FindExecutable, @@ -12,48 +11,6 @@ from launch_ros.substitutions import FindPackageShare -class GazeboMixin: - @staticmethod - def include_gazebo(**kwargs) -> IncludeLaunchDescription: - return IncludeLaunchDescription( - PythonLaunchDescriptionSource( - PathJoinSubstitution( - [ - FindPackageShare("gazebo_ros"), - "launch", - "gazebo.launch.py", - ] - ) - ), - **kwargs, - ) - - @staticmethod - def node_spawn_entity( - robot_name: Optional[Union[LaunchConfiguration, str]] = LaunchConfiguration( - "robot_name", default="lbr" - ), - tf: List[float] = [0.0, 0.0, 0.0, 0.0, 0.0, 0.0], - **kwargs, - ) -> Node: - label = ["-x", "-y", "-z", "-R", "-P", "-Y"] - tf = [str(x) for x in tf] - return Node( - package="gazebo_ros", - executable="spawn_entity.py", - arguments=[ - "-topic", - "robot_description", - "-entity", - robot_name, - ] - + [item for pair in zip(label, tf) for item in pair], - output="screen", - namespace=robot_name, - **kwargs, - ) - - class LBRDescriptionMixin: @staticmethod def param_robot_description( @@ -168,51 +125,3 @@ def node_static_tf( ], **kwargs, ) - - -class RVizMixin: - @staticmethod - def arg_rviz_config_pkg( - default_value: str = "lbr_description", - ) -> DeclareLaunchArgument: - return DeclareLaunchArgument( - name="rviz_config_pkg", - default_value=default_value, - description="The RViz configuration file.", - ) - - @staticmethod - def arg_rviz_config( - default_value: str = "config/config.rviz", - ) -> DeclareLaunchArgument: - return DeclareLaunchArgument( - name="rviz_config", - default_value=default_value, - description="The RViz configuration file.", - ) - - @staticmethod - def node_rviz( - rviz_config_pkg: Optional[ - Union[LaunchConfiguration, str] - ] = LaunchConfiguration("rviz_config_pkg", default="lbr_description"), - rviz_config: Optional[Union[LaunchConfiguration, str]] = LaunchConfiguration( - "rviz_config", default="config/config.rviz" - ), - **kwargs, - ) -> Node: - return Node( - package="rviz2", - executable="rviz2", - name="rviz2", - arguments=[ - "-d", - PathJoinSubstitution( - [ - FindPackageShare(rviz_config_pkg), - rviz_config, - ] - ), - ], - **kwargs, - ) diff --git a/lbr_bringup/lbr_bringup/gazebo.py b/lbr_bringup/lbr_bringup/gazebo.py new file mode 100644 index 00000000..145e052d --- /dev/null +++ b/lbr_bringup/lbr_bringup/gazebo.py @@ -0,0 +1,49 @@ +from typing import List, Optional, Union + +from launch.actions import IncludeLaunchDescription +from launch.launch_description_sources import PythonLaunchDescriptionSource +from launch.substitutions import LaunchConfiguration, PathJoinSubstitution +from launch_ros.actions import Node +from launch_ros.substitutions import FindPackageShare + + +class GazeboMixin: + @staticmethod + def include_gazebo(**kwargs) -> IncludeLaunchDescription: + return IncludeLaunchDescription( + PythonLaunchDescriptionSource( + PathJoinSubstitution( + [ + FindPackageShare("gazebo_ros"), + "launch", + "gazebo.launch.py", + ] + ) + ), + **kwargs, + ) + + @staticmethod + def node_spawn_entity( + robot_name: Optional[Union[LaunchConfiguration, str]] = LaunchConfiguration( + "robot_name", default="lbr" + ), + tf: List[float] = [0.0, 0.0, 0.0, 0.0, 0.0, 0.0], + **kwargs, + ) -> Node: + label = ["-x", "-y", "-z", "-R", "-P", "-Y"] + tf = [str(x) for x in tf] + return Node( + package="gazebo_ros", + executable="spawn_entity.py", + arguments=[ + "-topic", + "robot_description", + "-entity", + robot_name, + ] + + [item for pair in zip(label, tf) for item in pair], + output="screen", + namespace=robot_name, + **kwargs, + ) diff --git a/lbr_bringup/lbr_bringup/rviz.py b/lbr_bringup/lbr_bringup/rviz.py new file mode 100644 index 00000000..57f7ade7 --- /dev/null +++ b/lbr_bringup/lbr_bringup/rviz.py @@ -0,0 +1,54 @@ +from typing import Optional, Union + +from launch.actions import DeclareLaunchArgument +from launch.substitutions import LaunchConfiguration, PathJoinSubstitution +from launch_ros.actions import Node +from launch_ros.substitutions import FindPackageShare + + +class RVizMixin: + @staticmethod + def arg_rviz_config_pkg( + default_value: str = "lbr_bringup", + ) -> DeclareLaunchArgument: + return DeclareLaunchArgument( + name="rviz_config_pkg", + default_value=default_value, + description="The RViz configuration file.", + ) + + @staticmethod + def arg_rviz_config( + default_value: str = "config/mock.rviz", + ) -> DeclareLaunchArgument: + return DeclareLaunchArgument( + name="rviz_config", + default_value=default_value, + description="The RViz configuration file.", + ) + + @staticmethod + def node_rviz( + rviz_config_pkg: Optional[ + Union[LaunchConfiguration, str] + ] = LaunchConfiguration("rviz_config_pkg", default="lbr_bringup"), + rviz_config: Optional[Union[LaunchConfiguration, str]] = LaunchConfiguration( + "rviz_config", default="config/mock.rviz" + ), + **kwargs, + ) -> Node: + return Node( + package="rviz2", + executable="rviz2", + name="rviz2", + arguments=[ + "-d", + PathJoinSubstitution( + [ + FindPackageShare(rviz_config_pkg), + rviz_config, + ] + ), + ], + **kwargs, + ) diff --git a/lbr_demos/lbr_moveit_cpp/doc/lbr_moveit_cpp.rst b/lbr_demos/lbr_moveit_cpp/doc/lbr_moveit_cpp.rst index bf0fc249..ad38c5c6 100644 --- a/lbr_demos/lbr_moveit_cpp/doc/lbr_moveit_cpp.rst +++ b/lbr_demos/lbr_moveit_cpp/doc/lbr_moveit_cpp.rst @@ -69,7 +69,7 @@ The ``MoveGroup`` configurations are parsed conveniently through a mixin: .. code-block:: python - from launch_mixins.lbr_bringup import LBRMoveGroupMixin + from lbr_bringup.move_group import LBRMoveGroupMixin ... diff --git a/lbr_demos/lbr_moveit_cpp/launch/hello_moveit.launch.py b/lbr_demos/lbr_moveit_cpp/launch/hello_moveit.launch.py index 685b7014..765fecda 100644 --- a/lbr_demos/lbr_moveit_cpp/launch/hello_moveit.launch.py +++ b/lbr_demos/lbr_moveit_cpp/launch/hello_moveit.launch.py @@ -3,15 +3,19 @@ from launch import LaunchContext, LaunchDescription, LaunchDescriptionEntity from launch.actions import OpaqueFunction from launch.substitutions import LaunchConfiguration -from launch_mixins.lbr_bringup import LBRMoveGroupMixin -from launch_mixins.lbr_description import LBRDescriptionMixin from launch_ros.actions import Node +from lbr_bringup.description import LBRDescriptionMixin +from lbr_bringup.move_group import LBRMoveGroupMixin def launch_setup(context: LaunchContext) -> List[LaunchDescriptionEntity]: ld = LaunchDescription() model = LaunchConfiguration("model").perform(context) + mode = LaunchConfiguration("mode").perform(context) + use_sim_time = False + if mode == "gazebo": + use_sim_time = True # generate moveit configs moveit_configs = LBRMoveGroupMixin.moveit_configs_builder( @@ -26,7 +30,7 @@ def launch_setup(context: LaunchContext) -> List[LaunchDescriptionEntity]: executable="hello_moveit", parameters=[ moveit_configs.to_dict(), - {"use_sim_time": LaunchConfiguration("sim")}, + {"use_sim_time": use_sim_time}, LBRDescriptionMixin.param_robot_name(), ], ) @@ -39,7 +43,7 @@ def generate_launch_description() -> LaunchDescription: ld.add_action(LBRDescriptionMixin.arg_model()) ld.add_action(LBRDescriptionMixin.arg_robot_name()) - ld.add_action(LBRDescriptionMixin.arg_sim()) + ld.add_action(LBRDescriptionMixin.arg_mode()) ld.add_action(OpaqueFunction(function=launch_setup)) From ff8f2383c4f697c137a56953fda59ace1af13307 Mon Sep 17 00:00:00 2001 From: mhubii Date: Tue, 10 Sep 2024 11:58:34 +0200 Subject: [PATCH 05/15] bump version 2.0.0 -> 2.1.0 --- lbr_bringup/package.xml | 2 +- lbr_demos/lbr_demos_advanced_cpp/package.xml | 2 +- lbr_demos/lbr_demos_advanced_py/package.xml | 2 +- lbr_demos/lbr_demos_advanced_py/setup.py | 2 +- lbr_demos/lbr_demos_cpp/package.xml | 2 +- lbr_demos/lbr_demos_py/package.xml | 2 +- lbr_demos/lbr_demos_py/setup.py | 2 +- lbr_demos/lbr_moveit_cpp/package.xml | 2 +- lbr_description/package.xml | 2 +- lbr_fri_ros2/package.xml | 2 +- lbr_fri_ros2_stack/package.xml | 2 +- lbr_ros2_control/package.xml | 2 +- 12 files changed, 12 insertions(+), 12 deletions(-) diff --git a/lbr_bringup/package.xml b/lbr_bringup/package.xml index cfd6656a..342ef1ef 100644 --- a/lbr_bringup/package.xml +++ b/lbr_bringup/package.xml @@ -2,7 +2,7 @@ lbr_bringup - 2.0.0 + 2.1.0 LBR launch files. mhubii Apache License 2.0 diff --git a/lbr_demos/lbr_demos_advanced_cpp/package.xml b/lbr_demos/lbr_demos_advanced_cpp/package.xml index f8a74694..81e0b2e5 100644 --- a/lbr_demos/lbr_demos_advanced_cpp/package.xml +++ b/lbr_demos/lbr_demos_advanced_cpp/package.xml @@ -2,7 +2,7 @@ lbr_demos_advanced_cpp - 2.0.0 + 2.1.0 Advanced C++ demos for the lbr_ros2_control. mhubii Apache License 2.0 diff --git a/lbr_demos/lbr_demos_advanced_py/package.xml b/lbr_demos/lbr_demos_advanced_py/package.xml index cdd7ed21..ade1df84 100644 --- a/lbr_demos/lbr_demos_advanced_py/package.xml +++ b/lbr_demos/lbr_demos_advanced_py/package.xml @@ -2,7 +2,7 @@ lbr_demos_advanced_py - 2.0.0 + 2.1.0 Advanced Python demos for the lbr_ros2_control. mhubii cmower diff --git a/lbr_demos/lbr_demos_advanced_py/setup.py b/lbr_demos/lbr_demos_advanced_py/setup.py index 0a6e3644..d3dcdd20 100644 --- a/lbr_demos/lbr_demos_advanced_py/setup.py +++ b/lbr_demos/lbr_demos_advanced_py/setup.py @@ -6,7 +6,7 @@ setup( name=package_name, - version="2.0.0", + version="2.1.0", packages=[package_name], data_files=[ ("share/ament_index/resource_index/packages", ["resource/" + package_name]), diff --git a/lbr_demos/lbr_demos_cpp/package.xml b/lbr_demos/lbr_demos_cpp/package.xml index 24b6f2c3..f3d80e96 100644 --- a/lbr_demos/lbr_demos_cpp/package.xml +++ b/lbr_demos/lbr_demos_cpp/package.xml @@ -2,7 +2,7 @@ lbr_demos_cpp - 2.0.0 + 2.1.0 C++ demos for lbr_ros2_control. mhubii Apache License 2.0 diff --git a/lbr_demos/lbr_demos_py/package.xml b/lbr_demos/lbr_demos_py/package.xml index 67ddd0c4..6829f32e 100644 --- a/lbr_demos/lbr_demos_py/package.xml +++ b/lbr_demos/lbr_demos_py/package.xml @@ -2,7 +2,7 @@ lbr_demos_py - 2.0.0 + 2.1.0 Python demos for lbr_ros2_control. mhubii Apache License 2.0 diff --git a/lbr_demos/lbr_demos_py/setup.py b/lbr_demos/lbr_demos_py/setup.py index d79f3692..c3af4e8a 100644 --- a/lbr_demos/lbr_demos_py/setup.py +++ b/lbr_demos/lbr_demos_py/setup.py @@ -4,7 +4,7 @@ setup( name=package_name, - version="2.0.0", + version="2.1.0", packages=[package_name], data_files=[ ("share/ament_index/resource_index/packages", ["resource/" + package_name]), diff --git a/lbr_demos/lbr_moveit_cpp/package.xml b/lbr_demos/lbr_moveit_cpp/package.xml index 8d5f2804..dbda4e53 100644 --- a/lbr_demos/lbr_moveit_cpp/package.xml +++ b/lbr_demos/lbr_moveit_cpp/package.xml @@ -2,7 +2,7 @@ lbr_moveit_cpp - 2.0.0 + 2.1.0 Demo for using MoveIt C++ API. mhubii Apache-2.0 diff --git a/lbr_description/package.xml b/lbr_description/package.xml index f7445f5a..93829da1 100644 --- a/lbr_description/package.xml +++ b/lbr_description/package.xml @@ -2,7 +2,7 @@ lbr_description - 2.0.0 + 2.1.0 KUKA LBR description files mhubii Apache License 2.0 diff --git a/lbr_fri_ros2/package.xml b/lbr_fri_ros2/package.xml index a009f5d5..d5d2cc6c 100644 --- a/lbr_fri_ros2/package.xml +++ b/lbr_fri_ros2/package.xml @@ -2,7 +2,7 @@ lbr_fri_ros2 - 2.0.0 + 2.1.0 The lbr_fri_ros2 package provides the Fast Robot Interface (FRI) integration into ROS 2. Robot states can be extracted and commanded. mhubii diff --git a/lbr_fri_ros2_stack/package.xml b/lbr_fri_ros2_stack/package.xml index a072dd50..8025516c 100644 --- a/lbr_fri_ros2_stack/package.xml +++ b/lbr_fri_ros2_stack/package.xml @@ -2,7 +2,7 @@ lbr_fri_ros2_stack - 2.0.0 + 2.1.0 ROS 2 stack for KUKA LBRs. mhubii Apache License 2.0 diff --git a/lbr_ros2_control/package.xml b/lbr_ros2_control/package.xml index 57fd12f8..08e32b2f 100644 --- a/lbr_ros2_control/package.xml +++ b/lbr_ros2_control/package.xml @@ -2,7 +2,7 @@ lbr_ros2_control - 2.0.0 + 2.1.0 ROS 2 hardware hardware_interface for KUKA LBR through Fast Robot Interface (FRI). mhubii Apache License 2.0 From a1aa6f6b3165118130b54adc1ff40d82a83472c5 Mon Sep 17 00:00:00 2001 From: mhubii Date: Tue, 10 Sep 2024 12:11:44 +0200 Subject: [PATCH 06/15] modified doc --- CHANGELOG.rst | 1 + lbr_description/CMakeLists.txt | 2 +- lbr_fri_ros2_stack/doc/hardware_setup.rst | 5 +++-- 3 files changed, 5 insertions(+), 3 deletions(-) diff --git a/CHANGELOG.rst b/CHANGELOG.rst index 75448fe2..04523b81 100644 --- a/CHANGELOG.rst +++ b/CHANGELOG.rst @@ -13,6 +13,7 @@ Humble v2.1.0 (2024-09-10) * ``ros2 launch lbr_bringup hardware.launch.py`` (real robot) * ``ros2 launch lbr_bringup gazebo.launch.py`` (Gazebo simulation) * Added mock hardware to ``lbr_ros2_control`` (for simple ``ros2_control`` testing without the need for Gazebo) +* Minor documentation fixes Humble v2.0.0 (2024-07-08) -------------------------- diff --git a/lbr_description/CMakeLists.txt b/lbr_description/CMakeLists.txt index b2f9ac44..6debb078 100644 --- a/lbr_description/CMakeLists.txt +++ b/lbr_description/CMakeLists.txt @@ -11,7 +11,7 @@ find_package(ament_cmake_pytest REQUIRED) # install install( - DIRECTORY config gazebo meshes urdf + DIRECTORY gazebo meshes urdf DESTINATION share/${PROJECT_NAME} ) diff --git a/lbr_fri_ros2_stack/doc/hardware_setup.rst b/lbr_fri_ros2_stack/doc/hardware_setup.rst index 15754c42..c131be67 100644 --- a/lbr_fri_ros2_stack/doc/hardware_setup.rst +++ b/lbr_fri_ros2_stack/doc/hardware_setup.rst @@ -87,11 +87,12 @@ Install Applications to the Robot .. thumbnail:: img/computer/01_lbr_fri_ros2_create_package_name.png - #. .. dropdown:: Open a `Windows Terminal `_:octicon:`link-external` and clone the ``fri`` package + #. .. dropdown:: Open a `Windows Terminal `_:octicon:`link-external` and clone the ``fri`` package (make sure to replace ``FRI_CLIENT_VERSION=1.15`` with your FRI version, which must be available as branch `here `_:octicon:`link-external`) .. code-block:: bash - git clone https://github.com/lbr-stack/fri.git -b ros2-fri-1.15 $HOME\Downloads\fri + export FRI_CLIENT_VERSION=1.15 + git clone https://github.com/lbr-stack/fri.git -b fri-$FRI_CLIENT_VERSION $HOME\Downloads\fri #. .. dropdown:: Open a `Windows Terminal `_:octicon:`link-external` as ``Administrator`` and create a symbolic link to ``LBRServer.java`` From 6d62e2613240e11d3bf75e195a908219ce47747a Mon Sep 17 00:00:00 2001 From: mhubii Date: Tue, 10 Sep 2024 12:58:40 +0200 Subject: [PATCH 07/15] fixed doc --- lbr_fri_ros2_stack/doc/hardware_setup.rst | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/lbr_fri_ros2_stack/doc/hardware_setup.rst b/lbr_fri_ros2_stack/doc/hardware_setup.rst index c131be67..b7b63053 100644 --- a/lbr_fri_ros2_stack/doc/hardware_setup.rst +++ b/lbr_fri_ros2_stack/doc/hardware_setup.rst @@ -87,11 +87,11 @@ Install Applications to the Robot .. thumbnail:: img/computer/01_lbr_fri_ros2_create_package_name.png - #. .. dropdown:: Open a `Windows Terminal `_:octicon:`link-external` and clone the ``fri`` package (make sure to replace ``FRI_CLIENT_VERSION=1.15`` with your FRI version, which must be available as branch `here `_:octicon:`link-external`) + #. .. dropdown:: Open a `Windows Terminal `_:octicon:`link-external` and clone the ``fri`` package (make sure to replace ``$FRI_CLIENT_VERSION=1.15`` with your FRI version, which must be available as branch `here `_:octicon:`link-external`) .. code-block:: bash - export FRI_CLIENT_VERSION=1.15 + $FRI_CLIENT_VERSION=1.15 git clone https://github.com/lbr-stack/fri.git -b fri-$FRI_CLIENT_VERSION $HOME\Downloads\fri #. .. dropdown:: Open a `Windows Terminal `_:octicon:`link-external` as ``Administrator`` and create a symbolic link to ``LBRServer.java`` From f52daa96d2729a2abbe931e9ae78fc26c500b24e Mon Sep 17 00:00:00 2001 From: mhubii Date: Tue, 10 Sep 2024 14:42:21 +0200 Subject: [PATCH 08/15] updated doc --- CHANGELOG.rst | 4 +- README.md | 20 ++++--- docker/doc/docker.rst | 27 ++++++++-- lbr_bringup/launch/move_group.launch.py | 52 ++++++++++++++++--- lbr_bringup/launch/rviz.launch.py | 28 ++-------- lbr_bringup/lbr_bringup/description.py | 2 + lbr_bringup/lbr_bringup/rviz.py | 8 +++ .../launch/hello_moveit.launch.py | 4 +- 8 files changed, 99 insertions(+), 46 deletions(-) diff --git a/CHANGELOG.rst b/CHANGELOG.rst index 04523b81..0aaa52af 100644 --- a/CHANGELOG.rst +++ b/CHANGELOG.rst @@ -12,8 +12,8 @@ Humble v2.1.0 (2024-09-10) * ``ros2 launch lbr_bringup mock.launch.py`` (new: mock system) * ``ros2 launch lbr_bringup hardware.launch.py`` (real robot) * ``ros2 launch lbr_bringup gazebo.launch.py`` (Gazebo simulation) -* Added mock hardware to ``lbr_ros2_control`` (for simple ``ros2_control`` testing without the need for Gazebo) -* Minor documentation fixes +* Added mock hardware to ``lbr_ros2_control`` (for simple ``ros2_control`` testing without the need for Gazebo, refer https://control.ros.org/humble/doc/ros2_control/hardware_interface/doc/mock_components_userdoc.html) +* Updated documentation Humble v2.0.0 (2024-07-08) -------------------------- diff --git a/README.md b/README.md index ba546466..612c46f1 100644 --- a/README.md +++ b/README.md @@ -62,19 +62,25 @@ Full documentation available on [Read the Docs](https://lbr-stack.readthedocs.io colcon build --symlink-install ``` -4. Launch the simulation via +4. In terminal 1, launch a mock setup via ```shell source install/setup.bash - ros2 launch lbr_bringup bringup.launch.py \ - model:=iiwa7 # [iiwa7, iiwa14, med7, med14] \ - sim:=true # [true, false] \ - rviz:=true # [true, false] \ - moveit:=true # [true, false] + ros2 launch lbr_bringup mock.launch.py \ + model:=iiwa7 # [iiwa7, iiwa14, med7, med14] ``` > [!TIP] -> List all arguments for the launch file via `ros2 launch lbr_bringup bringup.launch.py -s` +> List all arguments for the launch file via `ros2 launch lbr_bringup mock.launch.py -s` + +5. In terminal 2, visualize the setup via + + ```shell + source install/setup.bash + ros2 launch lbr_bringup rviz.launch.py \ + rviz_config_pkg:=lbr_bringup \ + rviz_config:=config/mock.rviz + ``` Now, run the [demos](https://lbr-stack.readthedocs.io/en/latest/lbr_fri_ros2_stack/lbr_demos/doc/lbr_demos.html). To get started with the real robot, checkout the [Hardware Setup](https://lbr-stack.readthedocs.io/en/latest/lbr_fri_ros2_stack/lbr_fri_ros2_stack/doc/hardware_setup.html). diff --git a/docker/doc/docker.rst b/docker/doc/docker.rst index 9f894113..73759c9c 100644 --- a/docker/doc/docker.rst +++ b/docker/doc/docker.rst @@ -16,19 +16,20 @@ To run the ``lbr_fri_ros2_stack`` in a Docker container, follow the instructions mkdir -p lbr-stack/src && cd lbr-stack vcs import src --input https://raw.githubusercontent.com/lbr-stack/lbr_fri_ros2_stack/humble/lbr_fri_ros2_stack/repos-fri-${FRI_CLIENT_VERSION}.yaml +#. Install `Docker `_:octicon:`link-external`. + #. Copy the Dockerfile and the container scripts to the ``lbr-stack`` directory. Build and start the container .. code-block:: bash cp -r src/lbr_fri_ros2_stack/docker/* . - chmod +x container_build.sh - sudo ./container_build.sh + sudo ./container_build.sh # this will run the container once finished -#. Inside the container, launch e.g. simulation via (might require re-launch after Gazebo launched first time) +#. Once inside the container, launch e.g. the mock setup via .. code-block:: bash - ros2 launch lbr_bringup bringup.launch.py model:=iiwa7 sim:=true moveit:=true + ros2 launch lbr_bringup mock.launch.py model:=iiwa7 .. hint:: @@ -36,4 +37,20 @@ To run the ``lbr_fri_ros2_stack`` in a Docker container, follow the instructions .. code-block:: bash - ros2 launch lbr_bringup bringup.launch.py -s + ros2 launch lbr_bringup mock.launch.py + + .. hint:: + + Refer to :ref:`lbr_bringup` for more launch options. + +#. Connect another shell to the running container + + .. code-block:: bash + + ./container_new_console.sh + +#. Run e.g. MoveIt with RViz + + .. code-block:: bash + + ros2 launch lbr_bringup move_group.launch.py rviz:=true diff --git a/lbr_bringup/launch/move_group.launch.py b/lbr_bringup/launch/move_group.launch.py index c9796c9a..3d1f0bdd 100644 --- a/lbr_bringup/launch/move_group.launch.py +++ b/lbr_bringup/launch/move_group.launch.py @@ -2,14 +2,17 @@ from launch import LaunchContext, LaunchDescription, LaunchDescriptionEntity from launch.actions import OpaqueFunction -from launch.substitutions import LaunchConfiguration +from launch.conditions import IfCondition +from launch.substitutions import LaunchConfiguration, PathJoinSubstitution from lbr_bringup.description import LBRDescriptionMixin from lbr_bringup.move_group import LBRMoveGroupMixin +from lbr_bringup.rviz import RVizMixin -def launch_setup(context: LaunchContext) -> List[LaunchDescriptionEntity]: +def hidden_setup(context: LaunchContext) -> List[LaunchDescriptionEntity]: ld = LaunchDescription() + ld.add_action(LBRDescriptionMixin.arg_robot_name()) ld.add_action(LBRMoveGroupMixin.arg_allow_trajectory_execution()) ld.add_action(LBRMoveGroupMixin.arg_capabilities()) ld.add_action(LBRMoveGroupMixin.arg_disable_capabilities()) @@ -29,7 +32,7 @@ def launch_setup(context: LaunchContext) -> List[LaunchDescriptionEntity]: use_sim_time = True # MoveGroup - robot_name = LaunchConfiguration("robot_name").perform(context) + robot_name = LaunchConfiguration("robot_name") ld.add_action( LBRMoveGroupMixin.node_move_group( parameters=[ @@ -40,6 +43,43 @@ def launch_setup(context: LaunchContext) -> List[LaunchDescriptionEntity]: namespace=robot_name, ) ) + + # RViz if desired + rviz = RVizMixin.node_rviz( + rviz_config_pkg=f"{model}_moveit_config", + rviz_config="config/moveit.rviz", + parameters=LBRMoveGroupMixin.params_rviz( + moveit_configs=moveit_configs_builder.to_moveit_configs() + ) + + [{"use_sim_time": use_sim_time}], + remappings=[ + ( + "display_planned_path", + PathJoinSubstitution([robot_name, "display_planned_path"]), + ), + ("joint_states", PathJoinSubstitution([robot_name, "joint_states"])), + ( + "monitored_planning_scene", + PathJoinSubstitution([robot_name, "monitored_planning_scene"]), + ), + ("planning_scene", PathJoinSubstitution([robot_name, "planning_scene"])), + ( + "planning_scene_world", + PathJoinSubstitution([robot_name, "planning_scene_world"]), + ), + ( + "robot_description", + PathJoinSubstitution([robot_name, "robot_description"]), + ), + ( + "robot_description_semantic", + PathJoinSubstitution([robot_name, "robot_description_semantic"]), + ), + ], + condition=IfCondition(LaunchConfiguration("rviz")), + ) + + ld.add_action(rviz) return ld.entities @@ -48,9 +88,7 @@ def generate_launch_description() -> LaunchDescription: ld.add_action(LBRDescriptionMixin.arg_mode()) ld.add_action(LBRDescriptionMixin.arg_model()) - ld.add_action(LBRDescriptionMixin.arg_robot_name()) - ld.add_action(LBRDescriptionMixin.arg_mode()) - - ld.add_action(OpaqueFunction(function=launch_setup)) + ld.add_action(RVizMixin.arg_rviz()) + ld.add_action(OpaqueFunction(function=hidden_setup)) return ld diff --git a/lbr_bringup/launch/rviz.launch.py b/lbr_bringup/launch/rviz.launch.py index aed07293..cacc2faf 100644 --- a/lbr_bringup/launch/rviz.launch.py +++ b/lbr_bringup/launch/rviz.launch.py @@ -1,32 +1,14 @@ -from typing import List - -from launch import LaunchContext, LaunchDescription, LaunchDescriptionEntity -from launch.actions import OpaqueFunction -from launch.substitutions import LaunchConfiguration -from lbr_bringup.description import LBRDescriptionMixin +from launch import LaunchDescription from lbr_bringup.rviz import RVizMixin -def launch_setup(context: LaunchContext) -> List[LaunchDescriptionEntity]: - ld = LaunchDescription() - mode = LaunchConfiguration("mode").perform(context) - - # nodes - ld.add_action( - RVizMixin.node_rviz( - rviz_config_pkg="lbr_bringup", - rviz_config=f"config/{mode}.rviz", - ) - ) - return ld.entities - - def generate_launch_description() -> LaunchDescription: ld = LaunchDescription() # launch arguments - ld.add_action(LBRDescriptionMixin.arg_mode()) + ld.add_action(RVizMixin.arg_rviz_config()) + ld.add_action(RVizMixin.arg_rviz_config_pkg()) - # rviz with mode-specific configuration - ld.add_action(OpaqueFunction(function=launch_setup)) + # rviz + ld.add_action(RVizMixin.node_rviz()) return ld diff --git a/lbr_bringup/lbr_bringup/description.py b/lbr_bringup/lbr_bringup/description.py index 5981b84f..8d612d5a 100644 --- a/lbr_bringup/lbr_bringup/description.py +++ b/lbr_bringup/lbr_bringup/description.py @@ -110,6 +110,8 @@ def node_static_tf( child: Optional[Union[LaunchConfiguration, str]] = None, **kwargs, ) -> Node: + if len(tf) != 6: + raise ValueError("tf must be a list of 6 floats.") label = ["--x", "--y", "--z", "--roll", "--pitch", "--yaw"] tf = [str(x) for x in tf] return Node( diff --git a/lbr_bringup/lbr_bringup/rviz.py b/lbr_bringup/lbr_bringup/rviz.py index 57f7ade7..824bee74 100644 --- a/lbr_bringup/lbr_bringup/rviz.py +++ b/lbr_bringup/lbr_bringup/rviz.py @@ -7,6 +7,14 @@ class RVizMixin: + @staticmethod + def arg_rviz(default_value: str = "false") -> DeclareLaunchArgument: + return DeclareLaunchArgument( + name="rviz", + default_value=default_value, + description="Whether to launch RViz.", + ) + @staticmethod def arg_rviz_config_pkg( default_value: str = "lbr_bringup", diff --git a/lbr_demos/lbr_moveit_cpp/launch/hello_moveit.launch.py b/lbr_demos/lbr_moveit_cpp/launch/hello_moveit.launch.py index 765fecda..8debb47d 100644 --- a/lbr_demos/lbr_moveit_cpp/launch/hello_moveit.launch.py +++ b/lbr_demos/lbr_moveit_cpp/launch/hello_moveit.launch.py @@ -8,7 +8,7 @@ from lbr_bringup.move_group import LBRMoveGroupMixin -def launch_setup(context: LaunchContext) -> List[LaunchDescriptionEntity]: +def hidden_setup(context: LaunchContext) -> List[LaunchDescriptionEntity]: ld = LaunchDescription() model = LaunchConfiguration("model").perform(context) @@ -45,6 +45,6 @@ def generate_launch_description() -> LaunchDescription: ld.add_action(LBRDescriptionMixin.arg_robot_name()) ld.add_action(LBRDescriptionMixin.arg_mode()) - ld.add_action(OpaqueFunction(function=launch_setup)) + ld.add_action(OpaqueFunction(function=hidden_setup)) return ld From 9c2208728c3fa519215e6f6c97d64e027cc223be Mon Sep 17 00:00:00 2001 From: mhubii Date: Tue, 10 Sep 2024 14:43:43 +0200 Subject: [PATCH 09/15] fixed intent --- docker/doc/docker.rst | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/docker/doc/docker.rst b/docker/doc/docker.rst index 73759c9c..6b0a972f 100644 --- a/docker/doc/docker.rst +++ b/docker/doc/docker.rst @@ -39,7 +39,7 @@ To run the ``lbr_fri_ros2_stack`` in a Docker container, follow the instructions ros2 launch lbr_bringup mock.launch.py - .. hint:: + .. hint:: Refer to :ref:`lbr_bringup` for more launch options. From fbe0cc973fab8149eefd483f056e747bb55eac4d Mon Sep 17 00:00:00 2001 From: mhubii Date: Tue, 10 Sep 2024 14:44:58 +0200 Subject: [PATCH 10/15] fixed indentation --- docker/doc/docker.rst | 8 ++++---- 1 file changed, 4 insertions(+), 4 deletions(-) diff --git a/docker/doc/docker.rst b/docker/doc/docker.rst index 6b0a972f..53803077 100644 --- a/docker/doc/docker.rst +++ b/docker/doc/docker.rst @@ -45,12 +45,12 @@ To run the ``lbr_fri_ros2_stack`` in a Docker container, follow the instructions #. Connect another shell to the running container - .. code-block:: bash + .. code-block:: bash - ./container_new_console.sh + ./container_new_console.sh #. Run e.g. MoveIt with RViz - .. code-block:: bash + .. code-block:: bash - ros2 launch lbr_bringup move_group.launch.py rviz:=true + ros2 launch lbr_bringup move_group.launch.py rviz:=true From a0a2daf1bb238ad12b913fe80769caa2c066f385 Mon Sep 17 00:00:00 2001 From: mhubii Date: Tue, 10 Sep 2024 15:53:03 +0200 Subject: [PATCH 11/15] added doc --- lbr_bringup/doc/lbr_bringup.rst | 169 +++++++++++++++++++--- lbr_ros2_control/doc/lbr_ros2_control.rst | 8 +- 2 files changed, 154 insertions(+), 23 deletions(-) diff --git a/lbr_bringup/doc/lbr_bringup.rst b/lbr_bringup/doc/lbr_bringup.rst index dba75dfd..5ff6a1c3 100644 --- a/lbr_bringup/doc/lbr_bringup.rst +++ b/lbr_bringup/doc/lbr_bringup.rst @@ -1,38 +1,169 @@ lbr_bringup =========== -The ``lbr_bringup`` works for the simulation and the real robot. Run: +The ``lbr_bringup`` package hosts some launch files, which can be included via standard procedures: + +.. code:: python + + from launch import LaunchDescription + from launch.actions import IncludeLaunchDescription + from launch.launch_description_sources import PythonLaunchDescriptionSource + + def generate_launch_description() -> LaunchDescription: + ld = LaunchDescription() + ld.add_action( + IncludeLaunchDescription( + PythonLaunchDescriptionSource( + PathJoinSubstitution( + [ + FindPackageShare("lbr_bringup"), + "launch", + "mock.launch.py", + ] + ) + ), + ) + ) + return ld + +The launch files can also be run via the command line, as further described below. + +.. contents:: Table of Contents + :depth: 2 + :local: + :backlinks: none + +Launch Files +------------ +Launch a Mock Setup +~~~~~~~~~~ +Useful for running a physics-free simulation of the system. This launch file will (see `mock.launch.py `_:octicon:`link-external`): + +#. Run the ``robot_state_publisher`` +#. Run the ``ros2_control_node`` with mock components as loaded from ``robot_description`` +#. Load ``ros2_controllers`` .. code:: bash - ros2 launch lbr_bringup bringup.launch.py \ - model:=iiwa7 # [iiwa7, iiwa14, med7, med14] \ - sim:=true # [true, false] \ - rviz:=true # [true, false] \ - moveit:=true # [true, false] + ros2 launch lbr_bringup mock.launch.py \ + model:=iiwa7 # [iiwa7, iiwa14, med7, med14] .. note:: - For a list of available parameters, call ``ros2 launch lbr_bringup bringup.launch.py -s``. + List all arguments for the launch file via ``ros2 launch lbr_bringup mock.launch.py -s``. -When using the real robot, select ``sim:=false`` and +Launch the Gazebo Simulation +~~~~~~~~~~~~~~~~~~~~~~~~~~~~ +Useful for running a physics simulation the the system. This launch file will will (see `gazebo.launch.py `_:octicon:`link-external`): -.. dropdown:: Launch the ``LBRServer`` application on the ``KUKA smartPAD`` +#. Start the ``robot_state_publisher`` +#. Start the ``Gazebo`` simulation +#. Spawn the selected robot model (includes the ``ros2_control_node`` within the ``Gazebo`` plugin) +#. Load ``ros2_controllers`` - .. thumbnail:: ../../lbr_demos/doc/img/applications_lbr_server.png +.. code:: bash -Select + ros2 launch lbr_bringup gazebo.launch.py \ + model:=iiwa7 # [iiwa7, iiwa14, med7, med14] -- ``FRI send period``: ``10 ms`` -- ``IP address``: ``your configuration`` -- ``FRI control mode``: ``POSITION_CONTROL`` or ``JOINT_IMPEDANCE_CONTROL`` (will put the robot into a compliant mode) -- ``FRI client command mode``: ``POSITION`` +.. note:: + List all arguments for the launch file via ``ros2 launch lbr_bringup gazebo.launch.py -s``. -Users may also refer to :ref:`Software Architecture` for a better understanding of the underlying ``lbr_fri_ros2`` package. +Launch Hardware +~~~~~~~~~~~~~~~ +.. warning:: + Do always execute in ``T1`` mode first. .. note:: - For the real robot, make sure you have followed :doc:`Hardware Setup <../../lbr_fri_ros2_stack/doc/hardware_setup>` first. + Make sure you have followed :doc:`Hardware Setup <../../lbr_fri_ros2_stack/doc/hardware_setup>` first. -.. warning:: - On the real robot, do always execute in ``T1`` mode first. +#. Client side configurations: + + .. dropdown:: Launch the ``LBRServer`` application on the ``KUKA smartPAD`` + + .. thumbnail:: ../../lbr_demos/doc/img/applications_lbr_server.png + + Select + + - ``FRI send period``: ``10 ms`` + - ``IP address``: ``your configuration`` + - ``FRI control mode``: ``POSITION_CONTROL`` or ``JOINT_IMPEDANCE_CONTROL`` + - ``FRI client command mode``: ``POSITION`` + +#. Launch file: + + This launch file will (see `hardware.launch.py `_:octicon:`link-external`): + + #. Run the ``robot_state_publisher`` + #. Run the ``ros2_control_node`` with the ``lbr_fri_ros2::SystemInterface`` plugin from :doc:`lbr_ros2_control <../../lbr_ros2_control/doc/lbr_ros2_control>` as loaded from ``robot_description`` (which will attempt to establish a connection to the real robot). + #. Load ``ros2_controllers`` + + .. code:: bash + + ros2 launch lbr_bringup hardware.launch.py \ + model:=iiwa7 # [iiwa7, iiwa14, med7, med14] + + .. note:: + List all arguments for the launch file via ``ros2 launch lbr_bringup hardware.launch.py -s``. + +Launch RViz +~~~~~~~~~~~ +This launch file will spin up ``RViz`` for visualization. It will (see `rviz.launch.py `_:octicon:`link-external`): + +#. Read ``RViz`` configurations. +#. Run ``RViz``. + +.. code:: bash + + ros2 launch lbr_bringup rviz.launch.py \ + rviz_config_pkg:=lbr_bringup \ + rviz_config:=config/mock.rviz # [gazebo.rviz, hardware.rviz, mock.rviz] + +.. note:: + List all arguments for the launch file via ``ros2 launch lbr_bringup rviz.launch.py -s``. + +.. note:: + Requires the user to run :ref:`Launch a Mock Setup`, :ref:`Launch the Gazebo Simulation` or :ref:`Launch Hardware` first. + +Launch MoveIt +~~~~~~~~~~~~~ +Please note that MoveIt configurations are specific and you as a user will need to create your own for your system (potentially containing multiple robots or an end-effector). + +.. code:: bash + + ros2 launch lbr_bringup move_group.launch.py \ + model:=iiwa7 \ + mode:=mock \ + rviz:=true + +.. note:: + Requires the user to run :ref:`Launch a Mock Setup`, :ref:`Launch the Gazebo Simulation` or :ref:`Launch Hardware` first. + +.. note:: + Runs ``RViz`` with specific MoveIt configurations. + +Mixins +------ +The ``lbr_bringup`` package makes heavy use of mixins. Mixins are simply state-free classes with static methods. They are a convenient way of writing launch files. + +The below shows an example of the `rviz.launch.py `_:octicon:`link-external` file: + +.. code:: python + + from launch import LaunchDescription + from lbr_bringup.rviz import RVizMixin + + + def generate_launch_description() -> LaunchDescription: + ld = LaunchDescription() + + # launch arguments + ld.add_action(RVizMixin.arg_rviz_config()) + ld.add_action(RVizMixin.arg_rviz_config_pkg()) + + # rviz + ld.add_action(RVizMixin.node_rviz()) + return ld + +Which is quite compact and easy to read. General Information on the FRI ------------------------------ diff --git a/lbr_ros2_control/doc/lbr_ros2_control.rst b/lbr_ros2_control/doc/lbr_ros2_control.rst index 915dfcb0..36f2f7ba 100644 --- a/lbr_ros2_control/doc/lbr_ros2_control.rst +++ b/lbr_ros2_control/doc/lbr_ros2_control.rst @@ -34,18 +34,18 @@ The ``controller_manager::ControllerManager`` has a node, the `controller_manage Hardware Plugin --------------- -lbr_fri_ros2::SystemInterface +lbr_ros2_control::SystemInterface ~~~~~~~~~~~~~~~~~~~~~~~~~~~~~ -The ``lbr_fri_ros2::SystemInterface`` plugin implements a ``hardware_interface::SystemInterface`` and utilizes the :ref:`lbr_fri_ros2` package for communication with the robot. Overview :ref:`below ` (click to expand): +The ``lbr_ros2_control::SystemInterface`` plugin implements a ``hardware_interface::SystemInterface`` and utilizes the :ref:`lbr_fri_ros2` package for communication with the robot. Overview :ref:`below ` (click to expand): .. _lbr_ros2_control detailed software architecture figure: .. thumbnail:: img/lbr_ros2_control_detailed_v2.0.0.svg :alt: lbr_ros2_control_detailed - The ``controller_manager::ControllerManager`` loads the correct plugin from the ```` tag in the ``robot_description``: `lbr_system_interface.xacro `_:octicon:`link-external` -- FRI relevant parameters are forwarded to the ``lbr_fri_ros2::SystemInterface`` plugin from `lbr_system_paramters.yaml `_:octicon:`link-external` +- FRI relevant parameters are forwarded to the ``lbr_ros2_control::SystemInterface`` plugin from `lbr_system_paramters.yaml `_:octicon:`link-external` -The ``lbr_fri_ros2::SystemInterface`` is spun with the ``controller_manager`` node at a rate set by the ``update_rate`` parameter. The communication to the robot is run **asynchronously** at a rate set by the robot's sample time. +The ``lbr_ros2_control::SystemInterface`` is spun with the ``controller_manager`` node at a rate set by the ``update_rate`` parameter. The communication to the robot is run **asynchronously** at a rate set by the robot's sample time. **Why asynchronously**? KUKA designed the FRI that way, by adhering to this design choice, we can support multiple FRI versions, see :ref:`fri`! From d783f912672d160a1e60a46479d44074ad40f0fc Mon Sep 17 00:00:00 2001 From: mhubii Date: Tue, 10 Sep 2024 16:05:33 +0200 Subject: [PATCH 12/15] removed sim flag from doc --- lbr_bringup/doc/lbr_bringup.rst | 22 +++++++++---------- .../doc/lbr_demos_advanced_cpp.rst | 6 ++--- .../doc/lbr_demos_advanced_py.rst | 6 ++--- lbr_demos/lbr_demos_cpp/doc/lbr_demos_cpp.rst | 14 +++++------- lbr_demos/lbr_demos_py/doc/lbr_demos_py.rst | 14 +++++------- .../lbr_moveit_cpp/doc/lbr_moveit_cpp.rst | 15 +++++++++---- .../launch/hello_moveit.launch.py | 1 - lbr_demos/lbr_moveit_py/doc/lbr_moveit_py.rst | 15 +++++++++---- 8 files changed, 47 insertions(+), 46 deletions(-) diff --git a/lbr_bringup/doc/lbr_bringup.rst b/lbr_bringup/doc/lbr_bringup.rst index 5ff6a1c3..f09f17a4 100644 --- a/lbr_bringup/doc/lbr_bringup.rst +++ b/lbr_bringup/doc/lbr_bringup.rst @@ -34,7 +34,7 @@ The launch files can also be run via the command line, as further described belo Launch Files ------------ -Launch a Mock Setup +Mock Setup ~~~~~~~~~~ Useful for running a physics-free simulation of the system. This launch file will (see `mock.launch.py `_:octicon:`link-external`): @@ -50,8 +50,8 @@ Useful for running a physics-free simulation of the system. This launch file wil .. note:: List all arguments for the launch file via ``ros2 launch lbr_bringup mock.launch.py -s``. -Launch the Gazebo Simulation -~~~~~~~~~~~~~~~~~~~~~~~~~~~~ +Gazebo Simulation +~~~~~~~~~~~~~~~~~ Useful for running a physics simulation the the system. This launch file will will (see `gazebo.launch.py `_:octicon:`link-external`): #. Start the ``robot_state_publisher`` @@ -67,8 +67,8 @@ Useful for running a physics simulation the the system. This launch file will wi .. note:: List all arguments for the launch file via ``ros2 launch lbr_bringup gazebo.launch.py -s``. -Launch Hardware -~~~~~~~~~~~~~~~ +Hardware +~~~~~~~~ .. warning:: Do always execute in ``T1`` mode first. @@ -104,8 +104,8 @@ Launch Hardware .. note:: List all arguments for the launch file via ``ros2 launch lbr_bringup hardware.launch.py -s``. -Launch RViz -~~~~~~~~~~~ +RViz +~~~~ This launch file will spin up ``RViz`` for visualization. It will (see `rviz.launch.py `_:octicon:`link-external`): #. Read ``RViz`` configurations. @@ -121,10 +121,10 @@ This launch file will spin up ``RViz`` for visualization. It will (see `rviz.lau List all arguments for the launch file via ``ros2 launch lbr_bringup rviz.launch.py -s``. .. note:: - Requires the user to run :ref:`Launch a Mock Setup`, :ref:`Launch the Gazebo Simulation` or :ref:`Launch Hardware` first. + Requires the user to run `Mock Setup`_, `Gazebo Simulation`_ or `Launch Hardware`_ first. -Launch MoveIt -~~~~~~~~~~~~~ +MoveIt +~~~~~~ Please note that MoveIt configurations are specific and you as a user will need to create your own for your system (potentially containing multiple robots or an end-effector). .. code:: bash @@ -135,7 +135,7 @@ Please note that MoveIt configurations are specific and you as a user will need rviz:=true .. note:: - Requires the user to run :ref:`Launch a Mock Setup`, :ref:`Launch the Gazebo Simulation` or :ref:`Launch Hardware` first. + Requires the user to run `Mock Setup`_, `Gazebo Simulation`_ or `Launch Hardware`_ first. .. note:: Runs ``RViz`` with specific MoveIt configurations. diff --git a/lbr_demos/lbr_demos_advanced_cpp/doc/lbr_demos_advanced_cpp.rst b/lbr_demos/lbr_demos_advanced_cpp/doc/lbr_demos_advanced_cpp.rst index e253e146..2cd26566 100644 --- a/lbr_demos/lbr_demos_advanced_cpp/doc/lbr_demos_advanced_cpp.rst +++ b/lbr_demos/lbr_demos_advanced_cpp/doc/lbr_demos_advanced_cpp.rst @@ -34,8 +34,7 @@ This demo implements a simple admittance controller. .. code-block:: bash - ros2 launch lbr_bringup bringup.launch.py \ - sim:=false \ + ros2 launch lbr_bringup hardware.launch.py \ ctrl:=lbr_joint_position_command_controller \ model:=iiwa7 # [iiwa7, iiwa14, med7, med14] @@ -76,8 +75,7 @@ kinematics to move the robot's end-effector along the z-axis in Cartesian space. .. code-block:: bash - ros2 launch lbr_bringup bringup.launch.py \ - sim:=false \ + ros2 launch lbr_bringup hardware.launch.py \ ctrl:=lbr_joint_position_command_controller \ model:=iiwa7 # [iiwa7, iiwa14, med7, med14] diff --git a/lbr_demos/lbr_demos_advanced_py/doc/lbr_demos_advanced_py.rst b/lbr_demos/lbr_demos_advanced_py/doc/lbr_demos_advanced_py.rst index 2a70948d..fb37b371 100644 --- a/lbr_demos/lbr_demos_advanced_py/doc/lbr_demos_advanced_py.rst +++ b/lbr_demos/lbr_demos_advanced_py/doc/lbr_demos_advanced_py.rst @@ -34,8 +34,7 @@ This demo implements a simple admittance controller. .. code-block:: bash - ros2 launch lbr_bringup bringup.launch.py \ - sim:=false \ + ros2 launch lbr_bringup hardware.launch.py \ ctrl:=lbr_joint_position_command_controller \ model:=iiwa7 # [iiwa7, iiwa14, med7, med14] @@ -75,8 +74,7 @@ This demo implements an admittance controller with a remote center of motion (RC .. code-block:: bash - ros2 launch lbr_bringup bringup.launch.py \ - sim:=false \ + ros2 launch lbr_bringup hardware.launch.py \ ctrl:=lbr_joint_position_command_controller \ model:=iiwa7 # [iiwa7, iiwa14, med7, med14] diff --git a/lbr_demos/lbr_demos_cpp/doc/lbr_demos_cpp.rst b/lbr_demos/lbr_demos_cpp/doc/lbr_demos_cpp.rst index 1fda2817..dafaa698 100644 --- a/lbr_demos/lbr_demos_cpp/doc/lbr_demos_cpp.rst +++ b/lbr_demos/lbr_demos_cpp/doc/lbr_demos_cpp.rst @@ -34,9 +34,8 @@ This demo uses the :ref:`lbr_fri_ros2::LBRJointPositionCommandController` and ov .. code-block:: bash - ros2 launch lbr_bringup bringup.launch.py \ + ros2 launch lbr_bringup hardware.launch.py \ ctrl:=lbr_joint_position_command_controller \ - sim:=false \ model:=iiwa7 # [iiwa7, iiwa14, med7, med14] #. Run the `joint_sine_overlay `_:octicon:`link-external` node: @@ -57,8 +56,7 @@ Simulation .. code-block:: bash - ros2 launch lbr_bringup bringup.launch.py \ - sim:=true \ + ros2 launch lbr_bringup gazebo.launch.py \ ctrl:=joint_trajectory_controller \ model:=iiwa7 # [iiwa7, iiwa14, med7, med14] @@ -90,7 +88,7 @@ Hardware - ``FRI control mode``: ``POSITION_CONTROL`` or ``JOINT_IMPEDANCE_CONTROL`` - ``FRI client command mode``: ``POSITION`` -#. Proceed with steps 1 and 2 from `Simulation`_ but with ``sim:=false``. +#. Proceed with steps 1 and 2 from `Simulation`_ but with ``ros2 launch lbr_bringup hardware.launch.py``. LBR Torque Command Controller (Hardware only) --------------------------------------------- @@ -118,9 +116,8 @@ This demo uses the :ref:`lbr_fri_ros2::LBRTorqueCommandController` and overlays .. code-block:: bash - ros2 launch lbr_bringup bringup.launch.py \ + ros2 launch lbr_bringup hardware.launch.py \ ctrl:=lbr_torque_command_controller \ - sim:=false \ model:=iiwa7 # [iiwa7, iiwa14, med7, med14] #. Run the `torque_sine_overlay `_:octicon:`link-external` node: @@ -157,9 +154,8 @@ This demo uses the :ref:`lbr_fri_ros2::LBRWrenchCommandController` and overlays .. code-block:: bash - ros2 launch lbr_bringup bringup.launch.py \ + ros2 launch lbr_bringup hardware.launch.py \ ctrl:=lbr_wrench_command_controller \ - sim:=false \ model:=iiwa7 # [iiwa7, iiwa14, med7, med14] #. Run the `wrench_sine_overlay `_:octicon:`link-external` node: diff --git a/lbr_demos/lbr_demos_py/doc/lbr_demos_py.rst b/lbr_demos/lbr_demos_py/doc/lbr_demos_py.rst index 41e39938..c15ed340 100644 --- a/lbr_demos/lbr_demos_py/doc/lbr_demos_py.rst +++ b/lbr_demos/lbr_demos_py/doc/lbr_demos_py.rst @@ -34,9 +34,8 @@ This demo uses the :ref:`lbr_fri_ros2::LBRJointPositionCommandController` and ov .. code-block:: bash - ros2 launch lbr_bringup bringup.launch.py \ + ros2 launch lbr_bringup hardware.launch.py \ ctrl:=lbr_joint_position_command_controller \ - sim:=false \ model:=iiwa7 # [iiwa7, iiwa14, med7, med14] #. Run the `joint_sine_overlay `_:octicon:`link-external` node: @@ -57,8 +56,7 @@ Simulation .. code-block:: bash - ros2 launch lbr_bringup bringup.launch.py \ - sim:=true \ + ros2 launch lbr_bringup gazebo.launch.py \ ctrl:=joint_trajectory_controller \ model:=iiwa7 # [iiwa7, iiwa14, med7, med14] @@ -90,7 +88,7 @@ Hardware - ``FRI control mode``: ``POSITION_CONTROL`` or ``JOINT_IMPEDANCE_CONTROL`` - ``FRI client command mode``: ``POSITION`` -#. Proceed with steps 1 and 2 from `Simulation`_ but with ``sim:=false``. +#. Proceed with steps 1 and 2 from `Simulation`_ but with ``ros2 launch lbr_bringup hardware.launch.py``. LBR Torque Command Controller (Hardware only) --------------------------------------------- @@ -118,9 +116,8 @@ This demo uses the :ref:`lbr_fri_ros2::LBRTorqueCommandController` and overlays .. code-block:: bash - ros2 launch lbr_bringup bringup.launch.py \ + ros2 launch lbr_bringup hardware.launch.py \ ctrl:=lbr_torque_command_controller \ - sim:=false \ model:=iiwa7 # [iiwa7, iiwa14, med7, med14] #. Run the `torque_sine_overlay `_:octicon:`link-external` node: @@ -157,9 +154,8 @@ This demo uses the :ref:`lbr_fri_ros2::LBRWrenchCommandController` and overlays .. code-block:: bash - ros2 launch lbr_bringup bringup.launch.py \ + ros2 launch lbr_bringup hardware.launch.py \ ctrl:=lbr_wrench_command_controller \ - sim:=false \ model:=iiwa7 # [iiwa7, iiwa14, med7, med14] #. Run the `wrench_sine_overlay `_:octicon:`link-external` node: diff --git a/lbr_demos/lbr_moveit_cpp/doc/lbr_moveit_cpp.rst b/lbr_demos/lbr_moveit_cpp/doc/lbr_moveit_cpp.rst index ad38c5c6..d7b52b15 100644 --- a/lbr_demos/lbr_moveit_cpp/doc/lbr_moveit_cpp.rst +++ b/lbr_demos/lbr_moveit_cpp/doc/lbr_moveit_cpp.rst @@ -17,9 +17,16 @@ Simulation .. code-block:: bash - ros2 launch lbr_bringup bringup.launch.py \ + ros2 launch lbr_bringup mock.launch.py \ moveit:=true \ - sim:=true \ + model:=iiwa7 # [iiwa7, iiwa14, med7, med14] + +#. Run MoveIt: + + .. code-block:: bash + + ros2 launch lbr_moveit_cpp move_group.launch.py \ + mode:=mock \ model:=iiwa7 # [iiwa7, iiwa14, med7, med14] #. Run the `hello_moveit `_:octicon:`link-external` node: @@ -27,7 +34,7 @@ Simulation .. code-block:: bash ros2 launch lbr_moveit_cpp hello_moveit.launch.py \ - sim:=true \ + mode:=mock \ model:=iiwa7 # [iiwa7, iiwa14, med7, med14] Hardware @@ -50,7 +57,7 @@ Hardware - ``FRI control mode``: ``POSITION_CONTROL`` or ``JOINT_IMPEDANCE_CONTROL`` - ``FRI client command mode``: ``POSITION`` -#. Proceed with steps 1 and 2 from `Simulation`_ but with ``sim:=false``. +#. Proceed with steps 1, 2 and 3 from `Simulation`_ but with ``ros2 launch lbr_bringup hardware.launch.py``. Examining the Code ~~~~~~~~~~~~~~~~~~ diff --git a/lbr_demos/lbr_moveit_cpp/launch/hello_moveit.launch.py b/lbr_demos/lbr_moveit_cpp/launch/hello_moveit.launch.py index 8debb47d..fa080dfe 100644 --- a/lbr_demos/lbr_moveit_cpp/launch/hello_moveit.launch.py +++ b/lbr_demos/lbr_moveit_cpp/launch/hello_moveit.launch.py @@ -42,7 +42,6 @@ def generate_launch_description() -> LaunchDescription: ld = LaunchDescription() ld.add_action(LBRDescriptionMixin.arg_model()) - ld.add_action(LBRDescriptionMixin.arg_robot_name()) ld.add_action(LBRDescriptionMixin.arg_mode()) ld.add_action(OpaqueFunction(function=hidden_setup)) diff --git a/lbr_demos/lbr_moveit_py/doc/lbr_moveit_py.rst b/lbr_demos/lbr_moveit_py/doc/lbr_moveit_py.rst index d1358852..be501779 100644 --- a/lbr_demos/lbr_moveit_py/doc/lbr_moveit_py.rst +++ b/lbr_demos/lbr_moveit_py/doc/lbr_moveit_py.rst @@ -18,9 +18,16 @@ Simulation .. code-block:: bash - ros2 launch lbr_bringup bringup.launch.py \ - moveit:=true \ - sim:=true \ + ros2 launch lbr_bringup mock.launch.py \ + model:=iiwa7 # [iiwa7, iiwa14, med7, med14] + +#. Run MoveIt with RViz: + + .. code-block:: bash + + ros2 launch lbr_moveit_py move_group.launch.py \ + mode:=mock \ + rviz:=true \ model:=iiwa7 # [iiwa7, iiwa14, med7, med14] #. You can now move the robot via MoveIt in RViZ! @@ -45,4 +52,4 @@ Hardware - ``FRI control mode``: ``POSITION_CONTROL`` or ``JOINT_IMPEDANCE_CONTROL`` - ``FRI client command mode``: ``POSITION`` -#. Proceed with steps 1 and 2 from `Simulation`_ but with ``sim:=false``. +#. Proceed with steps 1 and 2 from `Simulation`_ but with ``ros2 launch lbr_bringup hardware.launch.py``. From a902b17544d54d0ba77baf5ce0a309c76e6c96e2 Mon Sep 17 00:00:00 2001 From: mhubii Date: Tue, 10 Sep 2024 16:08:45 +0200 Subject: [PATCH 13/15] fixed doc --- CHANGELOG.rst | 2 +- lbr_demos/lbr_moveit_py/doc/lbr_moveit_py.rst | 12 ++++++------ 2 files changed, 7 insertions(+), 7 deletions(-) diff --git a/CHANGELOG.rst b/CHANGELOG.rst index 0aaa52af..5ef0323a 100644 --- a/CHANGELOG.rst +++ b/CHANGELOG.rst @@ -61,7 +61,7 @@ Humble v1.4.2 (2023-12-29) Humble v1.4.1 (2023-12-15) -------------------------- * Removes the ``base_frame`` parameter from ``lbr_bringup``, ``lbr_description``, ``lbr_fri_ros2``, ``lbr_ros2_control`` -* Updates RViZ default config in ``lbr_moveit_config`` +* Updates RViz default config in ``lbr_moveit_config`` * Refers to https://github.com/lbr-stack/lbr_fri_ros2_stack/pull/144 Humble v1.4.0 (2023-12-08) diff --git a/lbr_demos/lbr_moveit_py/doc/lbr_moveit_py.rst b/lbr_demos/lbr_moveit_py/doc/lbr_moveit_py.rst index be501779..31654530 100644 --- a/lbr_demos/lbr_moveit_py/doc/lbr_moveit_py.rst +++ b/lbr_demos/lbr_moveit_py/doc/lbr_moveit_py.rst @@ -3,14 +3,14 @@ lbr_demos_moveit_py .. warning:: On hardware, do always execute in ``T1`` mode first. -MoveIt via RViZ +MoveIt via RViz ----------------- .. image:: img/iiwa7_moveit_rviz.png :align: center - :alt: MoveIt via RViZ -**IIWA 7 R800 in RViZ** + :alt: MoveIt via RViz +**IIWA 7 R800 in RViz** -To run MoveIt via RViZ, simply follow: +To run MoveIt via RViz, simply follow: Simulation ~~~~~~~~~~ @@ -25,12 +25,12 @@ Simulation .. code-block:: bash - ros2 launch lbr_moveit_py move_group.launch.py \ + ros2 launch lbr_bringup move_group.launch.py \ mode:=mock \ rviz:=true \ model:=iiwa7 # [iiwa7, iiwa14, med7, med14] -#. You can now move the robot via MoveIt in RViZ! +#. You can now move the robot via MoveIt in RViz! Hardware ~~~~~~~~ From 70a6f009b013ffda7905837c027d1052077856ca Mon Sep 17 00:00:00 2001 From: mhubii Date: Tue, 10 Sep 2024 16:10:15 +0200 Subject: [PATCH 14/15] fixed link --- lbr_bringup/doc/lbr_bringup.rst | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/lbr_bringup/doc/lbr_bringup.rst b/lbr_bringup/doc/lbr_bringup.rst index f09f17a4..da2c79d5 100644 --- a/lbr_bringup/doc/lbr_bringup.rst +++ b/lbr_bringup/doc/lbr_bringup.rst @@ -121,7 +121,7 @@ This launch file will spin up ``RViz`` for visualization. It will (see `rviz.lau List all arguments for the launch file via ``ros2 launch lbr_bringup rviz.launch.py -s``. .. note:: - Requires the user to run `Mock Setup`_, `Gazebo Simulation`_ or `Launch Hardware`_ first. + Requires the user to run `Mock Setup`_, `Gazebo Simulation`_ or `Hardware`_ first. MoveIt ~~~~~~ @@ -135,7 +135,7 @@ Please note that MoveIt configurations are specific and you as a user will need rviz:=true .. note:: - Requires the user to run `Mock Setup`_, `Gazebo Simulation`_ or `Launch Hardware`_ first. + Requires the user to run `Mock Setup`_, `Gazebo Simulation`_ or `Hardware`_ first. .. note:: Runs ``RViz`` with specific MoveIt configurations. From 677e8202373d7b9a91617fda6466413f94b024b7 Mon Sep 17 00:00:00 2001 From: mhubii Date: Tue, 10 Sep 2024 16:14:32 +0200 Subject: [PATCH 15/15] deleted legacy launch --- lbr_bringup/launch/bringup.launch.py | 62 ---------------------------- 1 file changed, 62 deletions(-) delete mode 100644 lbr_bringup/launch/bringup.launch.py diff --git a/lbr_bringup/launch/bringup.launch.py b/lbr_bringup/launch/bringup.launch.py deleted file mode 100644 index 8ae5f8a4..00000000 --- a/lbr_bringup/launch/bringup.launch.py +++ /dev/null @@ -1,62 +0,0 @@ -from launch import LaunchDescription -from launch.actions import IncludeLaunchDescription -from launch.conditions import LaunchConfigurationEquals -from launch.launch_description_sources import PythonLaunchDescriptionSource -from launch.substitutions import LaunchConfiguration, PathJoinSubstitution -from launch_ros.substitutions import FindPackageShare -from lbr_bringup.description import LBRDescriptionMixin - - -def generate_launch_description() -> LaunchDescription: - ld = LaunchDescription() - - ld.add_action(LBRDescriptionMixin.arg_mode()) - - ld.add_action( - IncludeLaunchDescription( - PythonLaunchDescriptionSource( - PathJoinSubstitution( - [ - FindPackageShare("lbr_bringup"), - "launch", - "hardware.launch.py", - ] - ) - ), - condition=LaunchConfigurationEquals( - LaunchConfiguration("mode"), "hardware" - ), - ) - ) - - ld.add_action( - IncludeLaunchDescription( - PythonLaunchDescriptionSource( - PathJoinSubstitution( - [ - FindPackageShare("lbr_bringup"), - "launch", - "mock.launch.py", - ] - ) - ), - condition=LaunchConfigurationEquals(LaunchConfiguration("mode"), "mock"), - ) - ) - - ld.add_action( - IncludeLaunchDescription( - PythonLaunchDescriptionSource( - PathJoinSubstitution( - [ - FindPackageShare("lbr_bringup"), - "launch", - "gazebo.launch.py", - ] - ) - ), - condition=LaunchConfigurationEquals(LaunchConfiguration("mode"), "gazebo"), - ) - ) - - return ld