From df10cb4df850cb5acf22d6400e7369ffe7cacfe2 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Denis=20=C5=A0togl?= Date: Sat, 1 May 2021 23:24:37 +0200 Subject: [PATCH] Extend controller templates with additional tests (#13) --- scripts/create-new-package.bash | 2 +- .../setup-controller-package.bash | 80 +++--- .../setup-hardware-interface-package.bash | 36 +-- .../ros2_control/controller/controller.cpp | 146 +++++++---- .../ros2_control/controller/controller.hpp | 24 +- .../controller/test_controller.cpp | 159 ++++++++++++ .../controller/test_controller.hpp | 237 ++++++++++++++++++ .../controller/test_load_controller.cpp | 14 +- .../hardware/robot_hardware_interface.cpp | 25 +- .../hardware/robot_hardware_interface.hpp | 13 +- .../ros2_control/hardware/robot_pluginlib.xml | 1 - .../test_robot_hardware_interface.cpp | 11 +- templates/ros2_control/robot_controllers.yaml | 3 +- .../ros2_control/robot_ros2_control.launch.py | 193 +++++++++----- ...test_forward_position_controller.launch.py | 36 +-- .../test_goal_publishers_config.yaml | 32 +++ ...test_joint_trajectory_controller.launch.py | 36 +-- 17 files changed, 765 insertions(+), 283 deletions(-) create mode 100644 templates/ros2_control/controller/test_controller.cpp create mode 100644 templates/ros2_control/controller/test_controller.hpp create mode 100644 templates/ros2_control/test_goal_publishers_config.yaml diff --git a/scripts/create-new-package.bash b/scripts/create-new-package.bash index d336fcbb..d2e7c233 100755 --- a/scripts/create-new-package.bash +++ b/scripts/create-new-package.bash @@ -96,7 +96,7 @@ read -p "If correct press , otherwise +C and start the script again if [[ $ros_version == 1 ]]; then - catkin_create_pkg $CREATE_PARAMS $PKG_NAME -V 0.0.1 -l $LICENSE -D "$PKG_DESCRIPTION" + catkin_create_pkg $CREATE_PARAMS $PKG_NAME -V 0.0.1 -l "$LICENSE" -D "$PKG_DESCRIPTION" catkin b -c source ~/.bashrc elif [[ $ros_version == 2 ]]; then diff --git a/scripts/ros2_control/setup-controller-package.bash b/scripts/ros2_control/setup-controller-package.bash index aa100de2..2f6cc8b9 100755 --- a/scripts/ros2_control/setup-controller-package.bash +++ b/scripts/ros2_control/setup-controller-package.bash @@ -18,7 +18,7 @@ usage="setup-controller-package.bash FILE_NAME [CLASS_NAME] [PKG_NAME]" # echo "" # echo "Your path is `pwd`. Is this your package folder where to setup robot's bringup?" -# read -p "If so press otherise +C and start the script again from the bringup folder." +# read -p "If so press otherwise +C and start the script again from the bringup folder." # Load Framework defines script_own_dir="$( cd "$( dirname "${BASH_SOURCE[0]}" )" > /dev/null && pwd )" @@ -30,7 +30,7 @@ if [ -z "$1" ]; then print_and_exit "ERROR: You should provide the file name!" "$usage" fi if [ -f src/$FILE_NAME.cpp ]; then - print_and_exit "ERROR:The file '$FILE_NAME' alread exist!" "$usage" + print_and_exit "ERROR:The file '$FILE_NAME' already exist!" "$usage" fi CLASS_NAME=$2 @@ -81,7 +81,7 @@ package_configured=${package_configured:="no"} echo "" echo "ATTENTION: Setting up ros2_control controller files with following parameters: file name '$FILE_NAME', class '$CLASS_NAME', package/namespace '$PKG_NAME'. Those will be placed in folder '`pwd`'." echo "" -read -p "If correct press , otherwise +C and start the script again from the package folder and/or with correct robot name." +read -p "If correct press , otherwise +C and start the script again from the package folder and/or with correct controller name." # Add folders if deleted ADD_FOLDERS=("include/$PKG_NAME" "src" "test") @@ -95,7 +95,9 @@ VC_H="include/$PKG_NAME/visibility_control.h" CTRL_HPP="include/$PKG_NAME/$FILE_NAME.hpp" CTRL_CPP="src/$FILE_NAME.cpp" PLUGIN_XML="$PKG_NAME.xml" -TEST_CPP="test/test_load_$FILE_NAME.cpp" +LOAD_TEST_CPP="test/test_load_$FILE_NAME.cpp" +TEST_CPP="test/test_$FILE_NAME.cpp" +TEST_HPP="test/test_$FILE_NAME.hpp" # Copy files if [[ ! -f "$VC_H" ]]; then @@ -104,18 +106,16 @@ fi cat $ROS2_CONTROL_CONTROLLER_TEMPLATES/controller_pluginlib.xml >> $PLUGIN_XML cp -n $ROS2_CONTROL_CONTROLLER_TEMPLATES/controller.hpp $CTRL_HPP cp -n $ROS2_CONTROL_CONTROLLER_TEMPLATES/controller.cpp $CTRL_CPP -cp -n $ROS2_CONTROL_CONTROLLER_TEMPLATES/test_load_controller.cpp $TEST_CPP +cp -n $ROS2_CONTROL_CONTROLLER_TEMPLATES/test_load_controller.cpp $LOAD_TEST_CPP +cp -n $ROS2_CONTROL_CONTROLLER_TEMPLATES/test_controller.cpp $TEST_CPP +cp -n $ROS2_CONTROL_CONTROLLER_TEMPLATES/test_controller.hpp $TEST_HPP echo "Template files copied." # TODO(anyone): fuse this with hardware interface package creating. # Add license header to the files -# TODO: When Propiatery then add the follwing before ament_lint_auto_find_test_dependencies() -# list(APPEND AMENT_LINT_AUTO_EXCLUDE -# ament_cmake_copyright -# ) -FILES_TO_LICENSE=("$CTRL_HPP" "$CTRL_CPP" "$TEST_CPP") +FILES_TO_LICENSE=("$CTRL_HPP" "$CTRL_CPP" "$LOAD_TEST_CPP" "$TEST_CPP" "$TEST_HPP") if [[ "$package_configured" == "no" ]]; then FILES_TO_LICENSE+=("$VC_H") fi @@ -174,17 +174,20 @@ echo ")" >> $TMP_FILE echo "target_include_directories(" >> $TMP_FILE echo " $PKG_NAME" >> $TMP_FILE echo " PUBLIC" >> $TMP_FILE -echo " include" >> $TMP_FILE +echo " $" >> $TMP_FILE +echo " $" >> $TMP_FILE echo ")" >> $TMP_FILE # TODO(anyone): Add this dependencies in a loop echo "ament_target_dependencies(" >> $TMP_FILE echo " $PKG_NAME" >> $TMP_FILE +echo " control_msgs" >> $TMP_FILE echo " controller_interface" >> $TMP_FILE echo " hardware_interface" >> $TMP_FILE echo " pluginlib" >> $TMP_FILE echo " rclcpp" >> $TMP_FILE echo " rclcpp_lifecycle" >> $TMP_FILE +echo " realtime_tools" >> $TMP_FILE echo ")" >> $TMP_FILE # TODO(anyone): Delete after Foxy!!! @@ -223,14 +226,25 @@ END_TEST_LINE=`tail -n +$TEST_LINE CMakeLists.txt | awk '$1 == "endif()" { print let CUT_LINE=$END_TEST_LINE-1 tail -n +$TEST_LINE CMakeLists.txt | head -$CUT_LINE >> $TMP_FILE +echo "" >> $TMP_FILE +echo " ament_add_gmock(test_load_$FILE_NAME $LOAD_TEST_CPP)" >> $TMP_FILE +echo " target_include_directories(test_load_$FILE_NAME PRIVATE include)" >> $TMP_FILE +echo " ament_target_dependencies(" >> $TMP_FILE +echo " test_load_$FILE_NAME" >> $TMP_FILE +echo " controller_manager" >> $TMP_FILE +echo " hardware_interface" >> $TMP_FILE +echo " ros2_control_test_assets" >> $TMP_FILE +echo " )" >> $TMP_FILE +echo "" + echo "" >> $TMP_FILE echo " ament_add_gmock(test_$FILE_NAME $TEST_CPP)" >> $TMP_FILE echo " target_include_directories(test_$FILE_NAME PRIVATE include)" >> $TMP_FILE +echo " target_link_libraries(test_$FILE_NAME $FILE_NAME)" >> $TMP_FILE echo " ament_target_dependencies(" >> $TMP_FILE echo " test_$FILE_NAME" >> $TMP_FILE -echo " controller_manager" >> $TMP_FILE +echo " controller_interface" >> $TMP_FILE echo " hardware_interface" >> $TMP_FILE -echo " ros2_control_test_assets" >> $TMP_FILE echo " )" >> $TMP_FILE echo "" @@ -250,11 +264,13 @@ if [[ "$package_configured" == "no" ]]; then # TODO(anyone): use this from a list so its the same as above echo "ament_export_dependencies(" >> $TMP_FILE + echo " control_msgs" >> $TMP_FILE echo " controller_interface" >> $TMP_FILE echo " hardware_interface" >> $TMP_FILE echo " pluginlib" >> $TMP_FILE echo " rclcpp" >> $TMP_FILE echo " rclcpp_lifecycle" >> $TMP_FILE + echo " realtime_tools" >> $TMP_FILE echo ")" >> $TMP_FILE fi @@ -266,7 +282,7 @@ tail -n +$TEST_LINE CMakeLists.txt | tail -n +$CUT_LINE >> $TMP_FILE mv $TMP_FILE CMakeLists.txt # CMakeLists.txt & package.xml: Add dependencies if they not exist -DEP_PKGS=("rclcpp_lifecycle" "rclcpp" "pluginlib" "hardware_interface" "controller_interface") +DEP_PKGS=("realtime_tools" "rclcpp_lifecycle" "rclcpp" "pluginlib" "hardware_interface" "controller_interface" "control_msgs") for DEP_PKG in "${DEP_PKGS[@]}"; do @@ -310,6 +326,10 @@ for DEP_PKG in "${TEST_DEP_PKGS[@]}"; do fi done +# Remove lint dependencies because they should be not included into build +sed -i "/ament_lint_auto_find_test_dependencies()/d" CMakeLists.txt +sed -i "/ament_lint_common<\/test_depend>/d" package.xml + # extend README with general instructions if [ -f README.md ]; then @@ -323,40 +343,10 @@ fi git add . # git commit -m "RosTeamWS: ros2_control skeleton files for $ROBOT_NAME generated." -ament_uncrustify --reformat +# ament_uncrustify --reformat # Compile and add new package the to the path compile_and_source_package $PKG_NAME "yes" echo "" echo "FINISHED: Your package is set and the tests should be finished without any errors." - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - diff --git a/scripts/ros2_control/setup-hardware-interface-package.bash b/scripts/ros2_control/setup-hardware-interface-package.bash index e1b33217..47a9c6c0 100755 --- a/scripts/ros2_control/setup-hardware-interface-package.bash +++ b/scripts/ros2_control/setup-hardware-interface-package.bash @@ -18,7 +18,7 @@ usage="setup-robot-ros2-control-hardware.bash FILE_NAME [CLASS_NAME] [PKG_NAME]" # echo "" # echo "Your path is `pwd`. Is this your package folder where to setup robot's bringup?" -# read -p "If so press otherise +C and start the script again from the bringup folder." +# read -p "If so press otherwise +C and start the script again from the bringup folder." # Load Framework defines script_own_dir="$( cd "$( dirname "${BASH_SOURCE[0]}" )" > /dev/null && pwd )" @@ -30,7 +30,7 @@ if [ -z "$1" ]; then print_and_exit "ERROR: You should provide the file name!" "$usage" fi if [ -f src/$FILE_NAME.cpp ]; then - print_and_exit "ERROR:The file '$FILE_NAME' alread exist!" "$usage" + print_and_exit "ERROR:The file '$FILE_NAME' already exist!" "$usage" fi CLASS_NAME=$2 @@ -120,7 +120,7 @@ cp -n $ROS2_CONTROL_HW_ITF_TEMPLATES/test_robot_hardware_interface.cpp $TEST_CPP echo "Template files copied." # Add license header to the files -# TODO: When Propiatery then add the follwing before ament_lint_auto_find_test_dependencies() +# TODO: When Propiatery then add the following before ament_lint_auto_find_test_dependencies() # list(APPEND AMENT_LINT_AUTO_EXCLUDE # ament_cmake_copyright # ) @@ -322,33 +322,3 @@ compile_and_source_package $PKG_NAME "yes" echo "" echo "FINISHED: Your package is set and the tests should be finished without any errors." - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - diff --git a/templates/ros2_control/controller/controller.cpp b/templates/ros2_control/controller/controller.cpp index b1101fe4..c51c754a 100644 --- a/templates/ros2_control/controller/controller.cpp +++ b/templates/ros2_control/controller/controller.cpp @@ -1,86 +1,112 @@ $LICENSE$ -#include "$package_name$/$file_name$.hpp" - +#include #include #include +#include "$package_name$/$file_name$.hpp" +#include "controller_interface/helpers.hpp" + namespace $package_name$ { +$ClassName$::$ClassName$() : controller_interface::ControllerInterface() +{ +} -$ClassName$::$ClassName$() -: controller_interface::ControllerInterface() -{} - -controller_interface::return_type -$ClassName$::init(const std::string & controller_name) +controller_interface::return_type $ClassName$::init(const std::string & controller_name) { auto ret = ControllerInterface::init(controller_name); - if (ret != controller_interface::return_type::SUCCESS) { + if (ret != controller_interface::return_type::OK) { return ret; } try { - auto node = get_node(); - node->declare_parameter>("joints", {}); - node->declare_parameter("interface_name", ""); + get_node()->declare_parameter>("joints", {}); + get_node()->declare_parameter("interface_name", ""); } catch (const std::exception & e) { fprintf(stderr, "Exception thrown during init stage with message: %s \n", e.what()); return controller_interface::return_type::ERROR; } - return controller_interface::return_type::SUCCESS; + return controller_interface::return_type::OK; } -CallbackReturn $ClassName$::on_configure( - const rclcpp_lifecycle::State & /*previous_state*/) +CallbackReturn $ClassName$::on_configure(const rclcpp_lifecycle::State & /*previous_state*/) { - joint_names_ = node_->get_parameter("joints").as_string_array(); - - if (joint_names_.empty()) { - RCLCPP_ERROR(get_node()->get_logger(), "'joints' parameter was empty"); + auto error_if_empty = [&](const auto & parameter, const char * parameter_name) { + if (parameter.empty()) { + RCLCPP_ERROR(get_node()->get_logger(), "'%s' parameter was empty", parameter_name); + return true; + } + return false; + }; + + auto get_string_array_param_and_error_if_empty = [&](std::vector & parameter, + const char * parameter_name) { + parameter = get_node()->get_parameter(parameter_name).as_string_array(); + return error_if_empty(parameter, parameter_name); + }; + + auto get_string_param_and_error_if_empty = [&](std::string & parameter, + const char * parameter_name) { + parameter = get_node()->get_parameter(parameter_name).as_string(); + return error_if_empty(parameter, parameter_name); + }; + + if (get_string_array_param_and_error_if_empty(joint_names_, "joints") || + get_string_param_and_error_if_empty(interface_name_, "interface_name")) + { return CallbackReturn::ERROR; } - // Specialized, child controllers set interfaces before calling init function. - if (interface_names_.empty()) { - interface_names_ = node_->get_parameter("interface_names").as_string_array(); - } + // Command Subscriber and callbacks + auto callback_command = [&](const std::shared_ptr msg) -> void { + if (msg->joint_names.size() == joint_names_.size()) { + input_command_.writeFromNonRT(msg); + } else { + RCLCPP_ERROR(get_node()->get_logger(), + "Received %zu , but expected %zu joints in command. Ignoring message.", + msg->joint_names.size(), joint_names_.size()); + } + }; + command_subscriber_ = get_node()->create_subscription( + "~/commands", rclcpp::SystemDefaultsQoS(), callback_command); - if (interface_names_.empty()) { - RCLCPP_ERROR(get_node()->get_logger(), "'interface_name' parameter was empty"); - return CallbackReturn::ERROR; - } + // State publisher + s_publisher_ = + get_node()->create_publisher("~/state", rclcpp::SystemDefaultsQoS()); + state_publisher_ = std::make_unique(s_publisher_); + + // TODO(anyone): Reserve memory in state publisher depending on the message type + state_publisher_->lock(); + state_publisher_->msg_.header.frame_id = joint_names_[0]; + state_publisher_->unlock(); RCLCPP_INFO_STREAM(get_node()->get_logger(), "configure successful"); return CallbackReturn::SUCCESS; } -controller_interface::InterfaceConfiguration -$ClassName$::command_interface_configuration() const +controller_interface::InterfaceConfiguration $ClassName$::command_interface_configuration() const { controller_interface::InterfaceConfiguration command_interfaces_config; command_interfaces_config.type = controller_interface::interface_configuration_type::INDIVIDUAL; + command_interfaces_config.names.reserve(joint_names_.size()); for (const auto & joint : joint_names_) { - for (const auto & interface : interface_names_) { - command_interfaces_config.names.push_back(joint + "/" + interface); - } + command_interfaces_config.names.push_back(joint + "/" + interface_name_); } return command_interfaces_config; } -controller_interface::InterfaceConfiguration -$ClassName$::state_interface_configuration() const +controller_interface::InterfaceConfiguration $ClassName$::state_interface_configuration() const { controller_interface::InterfaceConfiguration state_interfaces_config; state_interfaces_config.type = controller_interface::interface_configuration_type::INDIVIDUAL; + state_interfaces_config.names.reserve(joint_names_.size()); for (const auto & joint : joint_names_) { - for (const auto & interface : interface_names_) { - state_interfaces_config.names.push_back(joint + "/" + interface); - } + state_interfaces_config.names.push_back(joint + "/" + interface_name_); } return state_interfaces_config; @@ -88,15 +114,18 @@ controller_interface::InterfaceConfiguration // Fill ordered_interfaces with references to the matching interfaces // in the same order as in joint_names -template -bool get_ordered_interfaces( - std::vector & unordered_interfaces, const std::vector & joint_names, - const std::string & interface_type, std::vector> & ordered_interfaces) +// TODO(anyone): use the method from controller_interface/helpers.hpp when ros2_contol#370 +// is merged +template +bool get_ordered_interfaces(std::vector & unordered_interfaces, + const std::vector & joint_names, + const std::string & interface_type, + std::vector> & ordered_interfaces) { for (const auto & joint_name : joint_names) { for (auto & command_interface : unordered_interfaces) { if ((command_interface.get_name() == joint_name) && - (command_interface.get_interface_name() == interface_type)) + (command_interface.get_interface_name() == interface_type)) { ordered_interfaces.push_back(std::ref(command_interface)); } @@ -106,31 +135,44 @@ bool get_ordered_interfaces( return joint_names.size() == ordered_interfaces.size(); } -CallbackReturn $ClassName$::on_activate( - const rclcpp_lifecycle::State & /*previous_state*/) +CallbackReturn $ClassName$::on_activate(const rclcpp_lifecycle::State & /*previous_state*/) { + // Set default value in command + std::shared_ptr msg = std::make_shared(); + msg->joint_names = joint_names_; + msg->displacements.resize(joint_names_.size(), std::numeric_limits::quiet_NaN()); + input_command_.writeFromNonRT(msg); + return CallbackReturn::SUCCESS; } -CallbackReturn $ClassName$::on_deactivate( - const rclcpp_lifecycle::State & /*previous_state*/) +CallbackReturn $ClassName$::on_deactivate(const rclcpp_lifecycle::State & /*previous_state*/) { return CallbackReturn::SUCCESS; } controller_interface::return_type $ClassName$::update() { - for (auto index = 0ul; index < command_interfaces_.size(); ++index) { - command_interfaces_[index].set_value(state_interfaces_[index].get_value()); + auto current_command = input_command_.readFromRT(); + + for (size_t i = 0; i < command_interfaces_.size(); ++i) { + if (!std::isnan((*current_command)->displacements[i])) { + command_interfaces_[i].set_value((*current_command)->displacements[i]); + } + } + + if (state_publisher_ && state_publisher_->trylock()) { + state_publisher_->msg_.header.stamp = get_node()->now(); + state_publisher_->msg_.set_point = command_interfaces_[0].get_value(); + + state_publisher_->unlockAndPublish(); } - return controller_interface::return_type::SUCCESS; + return controller_interface::return_type::OK; } } // namespace $package_name$ #include "pluginlib/class_list_macros.hpp" -PLUGINLIB_EXPORT_CLASS( - $package_name$::$ClassName$, - controller_interface::ControllerInterface) +PLUGINLIB_EXPORT_CLASS($package_name$::$ClassName$, controller_interface::ControllerInterface) diff --git a/templates/ros2_control/controller/controller.hpp b/templates/ros2_control/controller/controller.hpp index 24a89777..d3d3855a 100644 --- a/templates/ros2_control/controller/controller.hpp +++ b/templates/ros2_control/controller/controller.hpp @@ -1,18 +1,21 @@ $LICENSE$ - #ifndef $PACKAGE_NAME$__$FILE_NAME$_HPP_ #define $PACKAGE_NAME$__$FILE_NAME$_HPP_ #include #include +#include "$package_name$/visibility_control.h" #include "controller_interface/controller_interface.hpp" #include "rclcpp_lifecycle/node_interfaces/lifecycle_node_interface.hpp" #include "rclcpp_lifecycle/state.hpp" +#include "realtime_tools/realtime_buffer.h" +#include "realtime_tools/realtime_publisher.h" - -#include "$package_name$/visibility_control.h" +// TODO(anyone): Replace with controller specific messages +#include "control_msgs/msg/joint_controller_state.hpp" +#include "control_msgs/msg/joint_jog.hpp" namespace $package_name$ { @@ -47,7 +50,20 @@ class $ClassName$ : public controller_interface::ControllerInterface protected: std::vector joint_names_; - std::vector interface_names_; + std::string interface_name_; + + // TODO(anyone): replace the state and command message types + // Command subscribers and Controller State publisher + using ControllerCommandMsg = control_msgs::msg::JointJog; + + rclcpp::Subscription::SharedPtr command_subscriber_ = nullptr; + realtime_tools::RealtimeBuffer> input_command_; + + using ControllerStateMsg = control_msgs::msg::JointControllerState; + using ControllerStatePublisher = realtime_tools::RealtimePublisher; + + rclcpp::Publisher::SharedPtr s_publisher_; + std::unique_ptr state_publisher_; }; } // namespace $package_name$ diff --git a/templates/ros2_control/controller/test_controller.cpp b/templates/ros2_control/controller/test_controller.cpp new file mode 100644 index 00000000..319e6459 --- /dev/null +++ b/templates/ros2_control/controller/test_controller.cpp @@ -0,0 +1,159 @@ +$LICENSE$ + +#include +#include +#include + +#include "test_$package_name$.hpp" + +// When there are many mandatory parameters, set all by default and remove one by one in a +// parameterized test +TEST_P($ClassName$TestParameterizedParameters, one_parameter_is_missing) +{ + SetUpController(); + + ASSERT_EQ(controller_->on_configure(rclcpp_lifecycle::State()), NODE_ERROR); +} + +// TODO(anyone): the new gtest version after 1.8.0 uses INSTANTIATE_TEST_SUITE_P +INSTANTIATE_TEST_CASE_P( + MissingMandatoryParameterDuringConfiguration, $ClassName$TestParameterizedParameters, + ::testing::Values(std::make_tuple(std::string("joints"), + rclcpp::ParameterValue(std::vector())), + std::make_tuple(std::string("interface_name"), rclcpp::ParameterValue("")))); + +TEST_F($ClassName$Test, joint_names_parameter_not_set) +{ + SetUpController(false); + + ASSERT_TRUE(controller_->joint_names_.empty()); + ASSERT_TRUE(controller_->interface_name_.empty()); + + controller_->get_node()->set_parameter({ "interface_name", interface_name_ }); + + ASSERT_EQ(controller_->on_configure(rclcpp_lifecycle::State()), NODE_ERROR); + + ASSERT_TRUE(controller_->joint_names_.empty()); + ASSERT_TRUE(controller_->interface_name_.empty()); +} + +TEST_F($ClassName$Test, interface_parameter_not_set) +{ + SetUpController(false); + + controller_->get_node()->set_parameter({ "joints", joint_names_ }); + + ASSERT_EQ(controller_->on_configure(rclcpp_lifecycle::State()), NODE_ERROR); + + ASSERT_TRUE(!controller_->joint_names_.empty()); + ASSERT_TRUE(controller_->interface_name_.empty()); +} + +TEST_F($ClassName$Test, all_parameters_set_configure_success) +{ + SetUpController(); + + ASSERT_TRUE(controller_->joint_names_.empty()); + ASSERT_TRUE(controller_->interface_name_.empty()); + + ASSERT_EQ(controller_->on_configure(rclcpp_lifecycle::State()), NODE_SUCCESS); + + ASSERT_TRUE(!controller_->joint_names_.empty()); + ASSERT_TRUE(controller_->joint_names_.size() == joint_names_.size()); + ASSERT_TRUE(std::equal(controller_->joint_names_.begin(), controller_->joint_names_.end(), + joint_names_.begin(), joint_names_.end())); + + ASSERT_TRUE(!controller_->interface_name_.empty()); + ASSERT_TRUE(controller_->interface_name_ == interface_name_); +} + +TEST_F($ClassName$Test, check_intefaces) +{ + SetUpController(); + + ASSERT_EQ(controller_->on_configure(rclcpp_lifecycle::State()), NODE_SUCCESS); + + auto command_intefaces = controller_->command_interface_configuration(); + ASSERT_EQ(command_intefaces.names.size(), joint_command_values_.size()); + + auto state_intefaces = controller_->state_interface_configuration(); + ASSERT_EQ(state_intefaces.names.size(), joint_state_values_.size()); +} + +TEST_F($ClassName$Test, activate_success) +{ + SetUpController(); + + ASSERT_EQ(controller_->on_configure(rclcpp_lifecycle::State()), NODE_SUCCESS); + ASSERT_EQ(controller_->on_activate(rclcpp_lifecycle::State()), NODE_SUCCESS); +} + +TEST_F($ClassName$Test, update_success) +{ + SetUpController(); + + ASSERT_EQ(controller_->on_configure(rclcpp_lifecycle::State()), NODE_SUCCESS); + ASSERT_EQ(controller_->on_activate(rclcpp_lifecycle::State()), NODE_SUCCESS); + ASSERT_EQ(controller_->update(), controller_interface::return_type::OK); +} + +TEST_F($ClassName$Test, deactivate_success) +{ + SetUpController(); + + ASSERT_EQ(controller_->on_configure(rclcpp_lifecycle::State()), NODE_SUCCESS); + ASSERT_EQ(controller_->on_activate(rclcpp_lifecycle::State()), NODE_SUCCESS); + ASSERT_EQ(controller_->on_deactivate(rclcpp_lifecycle::State()), NODE_SUCCESS); +} + +TEST_F($ClassName$Test, reactivate_success) +{ + SetUpController(); + + ASSERT_EQ(controller_->on_configure(rclcpp_lifecycle::State()), NODE_SUCCESS); + ASSERT_EQ(controller_->on_activate(rclcpp_lifecycle::State()), NODE_SUCCESS); + ASSERT_EQ(controller_->on_deactivate(rclcpp_lifecycle::State()), NODE_SUCCESS); + ASSERT_EQ(controller_->on_activate(rclcpp_lifecycle::State()), NODE_SUCCESS); + ASSERT_EQ(controller_->update(), controller_interface::return_type::OK); +} + +TEST_F($ClassName$Test, publish_status_success) +{ + SetUpController(); + + ASSERT_EQ(controller_->on_configure(rclcpp_lifecycle::State()), NODE_SUCCESS); + ASSERT_EQ(controller_->on_activate(rclcpp_lifecycle::State()), NODE_SUCCESS); + ASSERT_EQ(controller_->update(), controller_interface::return_type::OK); + + ControllerStateMsg msg; + subscribe_and_get_messages(msg); + + ASSERT_EQ(msg.set_point, 101.101); +} + +TEST_F($ClassName$Test, receive_message_and_publish_updated_status) +{ + SetUpController(); + rclcpp::executors::MultiThreadedExecutor executor; + executor.add_node(controller_->get_node()->get_node_base_interface()); + + ASSERT_EQ(controller_->on_configure(rclcpp_lifecycle::State()), NODE_SUCCESS); + ASSERT_EQ(controller_->on_activate(rclcpp_lifecycle::State()), NODE_SUCCESS); + ASSERT_EQ(controller_->update(), controller_interface::return_type::OK); + + ControllerStateMsg msg; + subscribe_and_get_messages(msg); + + ASSERT_EQ(msg.set_point, 101.101); + + publish_commands(); + ASSERT_TRUE(controller_->wait_for_commands(executor)); + + ASSERT_EQ(controller_->update(), controller_interface::return_type::OK); + + EXPECT_EQ(joint_command_values_[0], 0.45); + + subscribe_and_get_messages(msg); + + ASSERT_EQ(msg.set_point, 0.45); +} diff --git a/templates/ros2_control/controller/test_controller.hpp b/templates/ros2_control/controller/test_controller.hpp new file mode 100644 index 00000000..47342dcf --- /dev/null +++ b/templates/ros2_control/controller/test_controller.hpp @@ -0,0 +1,237 @@ +$LICENSE$ + +#ifndef TEST_$PACKAGE_NAME$_HPP_ +#define TEST_$PACKAGE_NAME$_HPP_ + +#include +#include +#include + +#include "$package_name$/$package_name$.hpp" +#include "gmock/gmock.h" +#include "hardware_interface/loaned_command_interface.hpp" +#include "hardware_interface/loaned_state_interface.hpp" +#include "hardware_interface/types/hardware_interface_return_values.hpp" +#include "rclcpp/parameter_value.hpp" +#include "rclcpp/utilities.hpp" +#include "rclcpp_lifecycle/node_interfaces/lifecycle_node_interface.hpp" + +// TODO(anyone): Replace with controller specific messages +#include "control_msgs/msg/joint_controller_state.hpp" +#include "control_msgs/msg/joint_jog.hpp" + +// TODO(anyone): replace the state and command message types +using ControllerStateMsg = control_msgs::msg::JointControllerState; +using ControllerCommandMsg = control_msgs::msg::JointJog; + +namespace +{ +constexpr auto NODE_SUCCESS = + rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn::SUCCESS; +constexpr auto NODE_ERROR = + rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn::ERROR; + +rclcpp::WaitResultKind wait_for(rclcpp::SubscriptionBase::SharedPtr subscription) +{ + rclcpp::WaitSet wait_set; + wait_set.add_subscription(subscription); + const auto timeout = std::chrono::seconds(10); + return wait_set.wait(timeout).kind(); +} + +} // namespace + +// subclassing and friending so we can access member variables +class Testable$ClassName$ : public $package_name$::$ClassName$ +{ + FRIEND_TEST($ClassName$Test, joint_names_parameter_not_set); + FRIEND_TEST($ClassName$Test, interface_parameter_not_set); + FRIEND_TEST($ClassName$Test, all_parameters_set_configure_success); + +public: + CallbackReturn on_configure(const rclcpp_lifecycle::State & previous_state) override + { + auto ret = $package_name$::$ClassName$::on_configure(previous_state); + // Only if on_configure is successful create subscription + if (ret == CallbackReturn::SUCCESS) { + command_subscriber_wait_set_.add_subscription(command_subscriber_); + } + return ret; + } + + /** + * @brief wait_for_command blocks until a new ControllerCommandMsg is received. + * Requires that the executor is not spinned elsewhere between the + * message publication and the call to this function. + * + * @return true if new ControllerCommandMsg msg was received, false if timeout. + */ + bool + wait_for_commands(rclcpp::Executor & executor, + const std::chrono::milliseconds & timeout = std::chrono::milliseconds{ 500 }) + { + bool success = + command_subscriber_wait_set_.wait(timeout).kind() == rclcpp::WaitResultKind::Ready; + if (success) { + executor.spin_some(); + } + return success; + } + +private: + rclcpp::WaitSet command_subscriber_wait_set_; +}; + +class $ClassName$Test : public ::testing::Test +{ +public: + static void SetUpTestCase() + { + rclcpp::init(0, nullptr); + } + + void SetUp() + { + // initialize controller + controller_ = std::make_unique(); + + command_publisher_node_ = std::make_shared("command_publisher"); + command_publisher_ = command_publisher_node_->create_publisher( + "/test_$package_name$/commands", rclcpp::SystemDefaultsQoS()); + } + + static void TearDownTestCase() + { + rclcpp::shutdown(); + } + + void TearDown() + { + controller_.reset(nullptr); + } + +protected: + void SetUpController(bool set_parameters = true) + { + const auto result = controller_->init("test_$package_name$"); + ASSERT_EQ(result, controller_interface::return_type::OK); + + std::vector command_ifs; + command_itfs_.reserve(joint_command_values_.size()); + command_ifs.reserve(joint_command_values_.size()); + + for (auto i = 0u; i < joint_command_values_.size(); ++i) { + command_itfs_.emplace_back(hardware_interface::CommandInterface( + joint_names_[i], interface_name_, &joint_command_values_[i])); + command_ifs.emplace_back(command_itfs_.back()); + } + // TODO(anyone): Add other command interfaces, if any + + std::vector state_ifs; + state_itfs_.reserve(joint_state_values_.size()); + state_ifs.reserve(joint_state_values_.size()); + + for (auto i = 0u; i < joint_state_values_.size(); ++i) { + state_itfs_.emplace_back(hardware_interface::StateInterface(joint_names_[i], interface_name_, + &joint_state_values_[i])); + state_ifs.emplace_back(state_itfs_.back()); + } + // TODO(anyone): Add other state interfaces, if any + + controller_->assign_interfaces(std::move(command_ifs), std::move(state_ifs)); + + if (set_parameters) { + controller_->get_node()->set_parameter({ "joints", joint_names_ }); + controller_->get_node()->set_parameter({ "interface_name", interface_name_ }); + } + } + + void subscribe_and_get_messages(ControllerStateMsg & msg) + { + // create a new subscriber + rclcpp::Node test_subscription_node("test_subscription_node"); + auto subs_callback = [&](const ControllerStateMsg::SharedPtr) {}; + auto subscription = test_subscription_node.create_subscription( + "/test_$package_name$/state", 10, subs_callback); + + // call update to publish the test value + ASSERT_EQ(controller_->update(), controller_interface::return_type::OK); + + // wait for message to be passed + ASSERT_EQ(wait_for(subscription), rclcpp::WaitResultKind::Ready); + + // take message from subscription + rclcpp::MessageInfo msg_info; + ASSERT_TRUE(subscription->take(msg, msg_info)); + } + + void publish_commands() + { + auto wait_for_topic = [&](const auto topic_name) { + size_t wait_count = 0; + while (command_publisher_node_->count_subscribers(topic_name) == 0) { + if (wait_count >= 5) { + auto error_msg = + std::string("publishing to ") + topic_name + " but no node subscribes to it"; + throw std::runtime_error(error_msg); + } + std::this_thread::sleep_for(std::chrono::milliseconds(100)); + ++wait_count; + } + }; + + wait_for_topic(command_publisher_->get_topic_name()); + + ControllerCommandMsg msg; + msg.joint_names = joint_names_; + msg.displacements = { 0.45 }; + msg.velocities = { 0.0 }; + msg.duration = 1.25; + + command_publisher_->publish(msg); + } + +protected: + // TODO(anyone): adjust the members as needed + + // Controller-related parameters + const std::vector joint_names_ = { "joint1" }; + const std::string interface_name_ = "my_interface"; + std::array joint_state_values_ = { 1.1 }; + std::array joint_command_values_ = { 101.101 }; + + std::vector state_itfs_; + std::vector command_itfs_; + + // Test related parameters + std::unique_ptr controller_; + rclcpp::Node::SharedPtr command_publisher_node_; + rclcpp::Publisher::SharedPtr command_publisher_; +}; + +// From the tutorial: https://www.sandordargo.com/blog/2019/04/24/parameterized-testing-with-gtest +class $ClassName$TestParameterizedParameters + : public $ClassName$Test, + public ::testing::WithParamInterface> +{ +public: + virtual void SetUp() + { + $ClassName$Test::SetUp(); + } + + static void TearDownTestCase() + { + $ClassName$Test::TearDownTestCase(); + } + +protected: + void SetUpController(bool set_parameters = true) + { + $ClassName$Test::SetUpController(set_parameters); + controller_->get_node()->undeclare_parameter(std::get<0>(GetParam())); + controller_->get_node()->declare_parameter(std::get<0>(GetParam()), std::get<1>(GetParam())); + } +}; + +#endif // TEST_$PACKAGE_NAME$_HPP_ diff --git a/templates/ros2_control/controller/test_load_controller.cpp b/templates/ros2_control/controller/test_load_controller.cpp index a0c4c9b6..4d579d53 100644 --- a/templates/ros2_control/controller/test_load_controller.cpp +++ b/templates/ros2_control/controller/test_load_controller.cpp @@ -1,6 +1,7 @@ $LICENSE$ #include + #include #include "controller_manager/controller_manager.hpp" @@ -15,14 +16,11 @@ TEST(TestLoad$ClassName$, load_controller) rclcpp::init(0, nullptr); std::shared_ptr executor = - std::make_shared(); + std::make_shared(); - controller_manager::ControllerManager cm( - std::make_unique( - ros2_control_test_assets::minimal_robot_urdf), executor, "test_controller_manager"); + controller_manager::ControllerManager cm(std::make_unique( + ros2_control_test_assets::minimal_robot_urdf), + executor, "test_controller_manager"); - ASSERT_NO_THROW( - cm.load_controller( - "test_$package_name$", - "$package_name$/$ClassName$")); + ASSERT_NO_THROW(cm.load_controller("test_$package_name$", "$package_name$/$ClassName$")); } diff --git a/templates/ros2_control/hardware/robot_hardware_interface.cpp b/templates/ros2_control/hardware/robot_hardware_interface.cpp index cecf046e..e0b65d9d 100644 --- a/templates/ros2_control/hardware/robot_hardware_interface.cpp +++ b/templates/ros2_control/hardware/robot_hardware_interface.cpp @@ -1,18 +1,15 @@ $LICENSE$ -#include "$package_name$/$file_name$.hpp" - #include #include +#include "$package_name$/$file_name$.hpp" #include "hardware_interface/types/hardware_interface_type_values.hpp" #include "rclcpp/rclcpp.hpp" namespace $package_name$ { - -hardware_interface::return_type $ClassName$::configure( - const hardware_interface::HardwareInfo & info) +hardware_interface::return_type $ClassName$::configure(const hardware_interface::HardwareInfo & info) { if (configure_default(info) != hardware_interface::return_type::OK) { return hardware_interface::return_type::ERROR; @@ -26,13 +23,11 @@ hardware_interface::return_type $ClassName$::configure( return hardware_interface::return_type::OK; } -std::vector -$ClassName$::export_state_interfaces() +std::vector $ClassName$::export_state_interfaces() { std::vector state_interfaces; for (uint i = 0; i < info_.joints.size(); i++) { - state_interfaces.emplace_back( - hardware_interface::StateInterface( + state_interfaces.emplace_back(hardware_interface::StateInterface( // TODO(anyone): insert correct interfaces info_.joints[i].name, hardware_interface::HW_IF_POSITION, &hw_states_[i])); } @@ -40,13 +35,11 @@ std::vector return state_interfaces; } -std::vector -$ClassName$::export_command_interfaces() +std::vector $ClassName$::export_command_interfaces() { std::vector command_interfaces; for (uint i = 0; i < info_.joints.size(); i++) { - command_interfaces.emplace_back( - hardware_interface::CommandInterface( + command_interfaces.emplace_back(hardware_interface::CommandInterface( // TODO(anyone): insert correct interfaces info_.joints[i].name, hardware_interface::HW_IF_POSITION, &hw_commands_[i])); } @@ -54,7 +47,6 @@ std::vector return command_interfaces; } - hardware_interface::return_type $ClassName$::start() { // TODO(anyone): prepare the robot to receive commands @@ -91,7 +83,4 @@ hardware_interface::return_type $ClassName$::write() #include "pluginlib/class_list_macros.hpp" -PLUGINLIB_EXPORT_CLASS( - $package_name$::$ClassName$, - hardware_interface::$Interface_Type$Interface -) +PLUGINLIB_EXPORT_CLASS($package_name$::$ClassName$, hardware_interface::$Interface_Type$Interface) diff --git a/templates/ros2_control/hardware/robot_hardware_interface.hpp b/templates/ros2_control/hardware/robot_hardware_interface.hpp index e015d9c3..0197fc1c 100644 --- a/templates/ros2_control/hardware/robot_hardware_interface.hpp +++ b/templates/ros2_control/hardware/robot_hardware_interface.hpp @@ -1,29 +1,24 @@ $LICENSE$ - #ifndef $PACKAGE_NAME$__$FILE_NAME$_HPP_ #define $PACKAGE_NAME$__$FILE_NAME$_HPP_ #include #include -#include "hardware_interface/base_interface.hpp" +#include "$package_name$/visibility_control.h" #include "hardware_interface/$interface_type$_interface.hpp" +#include "hardware_interface/base_interface.hpp" #include "hardware_interface/handle.hpp" #include "hardware_interface/hardware_info.hpp" #include "hardware_interface/types/hardware_interface_return_values.hpp" #include "hardware_interface/types/hardware_interface_status_values.hpp" - - -#include "$package_name$/visibility_control.h" - - #include "rclcpp/macros.hpp" namespace $package_name$ { -class $ClassName$ : public - hardware_interface::BaseInterface +class $ClassName$ + : public hardware_interface::BaseInterface { public: RCLCPP_SHARED_PTR_DEFINITIONS($ClassName$); diff --git a/templates/ros2_control/hardware/robot_pluginlib.xml b/templates/ros2_control/hardware/robot_pluginlib.xml index e9ac18a7..b11ef8d0 100644 --- a/templates/ros2_control/hardware/robot_pluginlib.xml +++ b/templates/ros2_control/hardware/robot_pluginlib.xml @@ -7,4 +7,3 @@ - diff --git a/templates/ros2_control/hardware/test_robot_hardware_interface.cpp b/templates/ros2_control/hardware/test_robot_hardware_interface.cpp index 1fdceffb..90aec053 100644 --- a/templates/ros2_control/hardware/test_robot_hardware_interface.cpp +++ b/templates/ros2_control/hardware/test_robot_hardware_interface.cpp @@ -1,11 +1,12 @@ $LICENSE$ #include + #include #include "hardware_interface/resource_manager.hpp" -#include "ros2_control_test_assets/descriptions.hpp" #include "ros2_control_test_assets/components_urdfs.hpp" +#include "ros2_control_test_assets/descriptions.hpp" class Test$ClassName$ : public ::testing::Test { @@ -14,7 +15,7 @@ class Test$ClassName$ : public ::testing::Test { // TODO(anyone): Extend this description to your robot $file_name$_2dof_ = - R"( + R"( $package_name$/$ClassName$ @@ -36,9 +37,9 @@ class Test$ClassName$ : public ::testing::Test std::string $file_name$_2dof_; }; -TEST_F(Test$ClassName$, load_$file_name$_2dof) { +TEST_F(Test$ClassName$, load_$file_name$_2dof) +{ auto urdf = - ros2_control_test_assets::urdf_head + $file_name$_2dof_ + - ros2_control_test_assets::urdf_tail; + ros2_control_test_assets::urdf_head + $file_name$_2dof_ + ros2_control_test_assets::urdf_tail; ASSERT_NO_THROW(hardware_interface::ResourceManager rm(urdf)); } diff --git a/templates/ros2_control/robot_controllers.yaml b/templates/ros2_control/robot_controllers.yaml index f5293eea..c26bffb3 100644 --- a/templates/ros2_control/robot_controllers.yaml +++ b/templates/ros2_control/robot_controllers.yaml @@ -52,5 +52,4 @@ joint_trajectory_controller: # delete entry if controller is no allow_partial_joints_goal: false # Defaults to false constraints: stopped_velocity_tolerance: 0.01 # Defaults to 0.01 - goal_time: 0.0 # Defaults to 0.0 (start immediatelly) - + goal_time: 0.0 # Defaults to 0.0 (start immediately) diff --git a/templates/ros2_control/robot_ros2_control.launch.py b/templates/ros2_control/robot_ros2_control.launch.py index b9d48fd9..00e0b472 100644 --- a/templates/ros2_control/robot_ros2_control.launch.py +++ b/templates/ros2_control/robot_ros2_control.launch.py @@ -1,9 +1,8 @@ -$LICENSE$ +# $LICENSE$ from launch import LaunchDescription -from launch.actions import DeclareLaunchArgument, ExecuteProcess +from launch.actions import DeclareLaunchArgument from launch.substitutions import Command, FindExecutable, LaunchConfiguration, PathJoinSubstitution - from launch_ros.actions import Node from launch_ros.substitutions import FindPackageShare @@ -11,94 +10,150 @@ def generate_launch_description(): # Declare arguments declared_arguments = [] - declared_arguments.append(DeclareLaunchArgument( - 'controllers_package', default_value='ros2_control_demo_robot', - description='Package with the controller\'s configuration in "config" folder. \ - Usually the argument is not set, it enables use of a custom description.')) - declared_arguments.append(DeclareLaunchArgument( - 'description_package', default_value='ros2_control_demo_robot', - description='Description package with robot URDF/xacro files. Usually the argument \ - is not set, it enables use of a custom description.')) - declared_arguments.append(DeclareLaunchArgument( - 'prefix', default_value='""', description='Prefix of the joint names, useful for \ - multi-robot setup. If changed than also joint names in the controllers\' configuration \ - have to be updated.')) - declared_arguments.append(DeclareLaunchArgument( - 'use_fake_hardware', default_value='true', - description='Start robot with fake hardware mirroring command to its states.')) - declared_arguments.append(DeclareLaunchArgument( - 'fake_sensor_commands', default_value='false', - description='Enable fake command interfaces for sensors used for simple simulations. \ - Used only if \'use_fake_hardware\' parameter is true.')) + declared_arguments.append( + DeclareLaunchArgument( + "runtime_config_package", + default_value="$RUNTIME_CONFIG_PKG_NAME$", + description='Package with the controller\'s configuration in "config" folder. \ + Usually the argument is not set, it enables use of a custom setup.', + ) + ) + declared_arguments.append( + DeclareLaunchArgument( + "controllers_file", + default_value="$ROBOT_NAME$_controllers.yaml", + description="YAML file with the controllers configuration.", + ) + ) + declared_arguments.append( + DeclareLaunchArgument( + "description_package", + default_value="$DESCR_PKG_NAME$", + description="Description package with robot URDF/xacro files. Usually the argument \ + is not set, it enables use of a custom description.", + ) + ) + declared_arguments.append( + DeclareLaunchArgument( + "description_file", + default_value="$ROBOT_NAME$.urdf.xacro", + description="URDF/XACRO description file with the robot.", + ) + ) + declared_arguments.append( + DeclareLaunchArgument( + "prefix", + default_value='""', + description="Prefix of the joint names, useful for \ + multi-robot setup. If changed than also joint names in the controllers' configuration \ + have to be updated.", + ) + ) + declared_arguments.append( + DeclareLaunchArgument( + "use_fake_hardware", + default_value="true", + description="Start robot with fake hardware mirroring command to its states.", + ) + ) + declared_arguments.append( + DeclareLaunchArgument( + "fake_sensor_commands", + default_value="false", + description="Enable fake command interfaces for sensors used for simple simulations. \ + Used only if 'use_fake_hardware' parameter is true.", + ) + ) + declared_arguments.append( + DeclareLaunchArgument( + "robot_controller", + default_value="forward_position_controller", + description="Robot controller to start.", + ) + ) # Initialize Arguments - controllers_package = LaunchConfiguration('controllers_package') - description_package = LaunchConfiguration('description_package') - prefix = LaunchConfiguration('prefix') - use_fake_hardware = LaunchConfiguration('use_fake_hardware') - fake_sensor_commands = LaunchConfiguration('fake_sensor_commands') + runtime_config_package = LaunchConfiguration("runtime_config_package") + controllers_file = LaunchConfiguration("controllers_file") + description_package = LaunchConfiguration("description_package") + description_file = LaunchConfiguration("description_file") + prefix = LaunchConfiguration("prefix") + use_fake_hardware = LaunchConfiguration("use_fake_hardware") + fake_sensor_commands = LaunchConfiguration("fake_sensor_commands") + robot_controller = LaunchConfiguration("robot_controller") # Get URDF via xacro - robot_description_content = Command([ - PathJoinSubstitution([FindExecutable(name='xacro')]), - ' ', - PathJoinSubstitution([FindPackageShare(description_package), - 'description', '$ROBOT_NAME$.urdf.xacro']), - ' ', - 'prefix:=', prefix, ' ', - 'use_fake_hardware:=', use_fake_hardware, ' ', - 'fake_sensor_commands:=', fake_sensor_commands, ' ', - ]) + robot_description_content = Command( + [ + PathJoinSubstitution([FindExecutable(name="xacro")]), + " ", + PathJoinSubstitution( + [FindPackageShare(description_package), "description", description_file] + ), + " ", + "prefix:=", + prefix, + " ", + "use_fake_hardware:=", + use_fake_hardware, + " ", + "fake_sensor_commands:=", + fake_sensor_commands, + " ", + ] + ) - robot_description = {'robot_description': robot_description_content} + robot_description = {"robot_description": robot_description_content} - robot_controllers = PathJoinSubstitution([ - FindPackageShare(controllers_package), - 'config', - '$ROBOT_NAME$_controllers.yaml' - ]) - rviz_config_file = PathJoinSubstitution([ - FindPackageShare(description_package), - 'rviz', - '$ROBOT_NAME$.rviz' - ]) + robot_controllers = PathJoinSubstitution( + [FindPackageShare(runtime_config_package), "config", controllers_file] + ) + rviz_config_file = PathJoinSubstitution( + [FindPackageShare(description_package), "rviz", "$ROBOT_NAME$.rviz"] + ) control_node = Node( - package='controller_manager', - executable='ros2_control_node', + package="controller_manager", + executable="ros2_control_node", parameters=[robot_description, robot_controllers], output={ - 'stdout': 'screen', - 'stderr': 'screen', + "stdout": "screen", + "stderr": "screen", }, ) robot_state_pub_node = Node( - package='robot_state_publisher', - executable='robot_state_publisher', - output='both', - parameters=[robot_description] + package="robot_state_publisher", + executable="robot_state_publisher", + output="both", + parameters=[robot_description], ) rviz_node = Node( - package='rviz2', - executable='rviz2', - name='rviz2', - output='log', - arguments=['-d', rviz_config_file], + package="rviz2", + executable="rviz2", + name="rviz2", + output="log", + arguments=["-d", rviz_config_file], + ) + + joint_state_controller_spawner = Node( + package="controller_manager", + executable="spawner.py", + arguments=["joint_state_controller", "--controller-manager", "/controller_manager"], ) - robot_controller = 'forward_position_controller' - # robot_controller = 'joint_trajectory_controller' robot_controller_spawner = Node( - package='controller_manager', - executable='spawner.py', - arguments=[robot_controller, '-c', '/controller_manager']) + package="controller_manager", + executable="spawner.py", + arguments=[robot_controller, "-c", "/controller_manager"], + ) return LaunchDescription( - declared_arguments + - [ + declared_arguments + + [ control_node, robot_state_pub_node, rviz_node, joint_state_controller_spawner, robot_controller_spawner, - ]) + ] + ) diff --git a/templates/ros2_control/test_forward_position_controller.launch.py b/templates/ros2_control/test_forward_position_controller.launch.py index e6f5b6d8..21e233e8 100644 --- a/templates/ros2_control/test_forward_position_controller.launch.py +++ b/templates/ros2_control/test_forward_position_controller.launch.py @@ -1,4 +1,4 @@ -$LICENSE$ +# $LICENSE$ from launch import LaunchDescription from launch.substitutions import PathJoinSubstitution @@ -8,21 +8,21 @@ def generate_launch_description(): - position_goals = PathJoinSubstitution([ - FindPackageShare('$PKG_NAME$'), - 'config', - '$ROBOT_NAME$_forward_position_publisher.yaml' - ]) + position_goals = PathJoinSubstitution( + [FindPackageShare("$PKG_NAME$"), "config", "test_goal_publishers_config.yaml"] + ) - return LaunchDescription([ - Node( - package='ros2_control_test_nodes', - executable='publisher_forward_position_controller', - parameters=[position_goals], - output={ - 'stdout': 'screen', - 'stderr': 'screen', - }, - ) - - ]) + return LaunchDescription( + [ + Node( + package="ros2_control_test_nodes", + executable="publisher_forward_position_controller", + name="publisher_forward_position_controller", + parameters=[position_goals], + output={ + "stdout": "screen", + "stderr": "screen", + }, + ) + ] + ) diff --git a/templates/ros2_control/test_goal_publishers_config.yaml b/templates/ros2_control/test_goal_publishers_config.yaml new file mode 100644 index 00000000..444c1958 --- /dev/null +++ b/templates/ros2_control/test_goal_publishers_config.yaml @@ -0,0 +1,32 @@ +publisher_forward_position_controller: + ros__parameters: + + controller_name: "forward_position_controller" + wait_sec_between_publish: 5 + + goal_names: ["pos1", "pos2", "pos3", "pos4"] + pos1: [0.785, 0.785, 0.785, 0.785, 0.785, 0.785] + pos2: [0, 0, 0, 0, 0, 0] + pos3: [-0.785, -0.785, -0.785, -0.785, -0.785, -0.785] + pos4: [0, 0, 0, 0, 0, 0] + + +publisher_joint_trajectory_controller: + ros__parameters: + + controller_name: "joint_trajectory_controller" + wait_sec_between_publish: 6 + + goal_names: ["pos1", "pos2", "pos3", "pos4"] + pos1: [0.785, 0.785, 0.785, 0.785, 0.785, 0.785] + pos2: [0, 0, 0, 0, 0, 0] + pos3: [-0.785, -0.785, -0.785, -0.785, -0.785, -0.785] + pos4: [0, 0, 0, 0, 0, 0] + + joints: + - joint1 + - joint2 + - joint3 + - joint4 + - joint5 + - joint6 diff --git a/templates/ros2_control/test_joint_trajectory_controller.launch.py b/templates/ros2_control/test_joint_trajectory_controller.launch.py index 6d70cef7..80fbb706 100644 --- a/templates/ros2_control/test_joint_trajectory_controller.launch.py +++ b/templates/ros2_control/test_joint_trajectory_controller.launch.py @@ -1,4 +1,4 @@ -$LICENSE$ +# $LICENSE$ from launch import LaunchDescription from launch.substitutions import PathJoinSubstitution @@ -8,21 +8,21 @@ def generate_launch_description(): - position_goals = PathJoinSubstitution([ - FindPackageShare('$PKG_NAME$'), - 'config', - '$ROBOT_NAME$_joint_trajectory_publisher.yaml' - ]) + position_goals = PathJoinSubstitution( + [FindPackageShare("$PKG_NAME$"), "config", "test_goal_publishers_config.yaml"] + ) - return LaunchDescription([ - Node( - package='ros2_control_test_nodes', - executable='publisher_joint_trajectory_controller', - parameters=[position_goals], - output={ - 'stdout': 'screen', - 'stderr': 'screen', - }, - ) - - ]) + return LaunchDescription( + [ + Node( + package="ros2_control_test_nodes", + executable="publisher_joint_trajectory_controller", + name="publisher_joint_trajectory_controller", + parameters=[position_goals], + output={ + "stdout": "screen", + "stderr": "screen", + }, + ) + ] + )