diff --git a/.github/workflows/humble-debian-build.yml b/.github/workflows/humble-debian-build.yml new file mode 100644 index 0000000000..0db1f58210 --- /dev/null +++ b/.github/workflows/humble-debian-build.yml @@ -0,0 +1,30 @@ +name: Debian Humble Build +on: + workflow_dispatch: + pull_request: + branches: + - humble + schedule: + # Run every day to detect flakiness and broken dependencies + - cron: '03 1 * * *' + + +jobs: + humble_debian: + name: Humble debian build + runs-on: ubuntu-latest + env: + ROS_DISTRO: humble + container: ghcr.io/ros-controls/ros:humble-debian + steps: + - uses: actions/checkout@v4 + with: + path: src/ros2_control + - name: Build and test + shell: bash + run: | + source /opt/ros2_ws/install/setup.bash + vcs import src < src/ros2_control/ros2_control.${{ env.ROS_DISTRO }}.repos + colcon build --packages-skip rqt_controller_manager + colcon test --packages-skip rqt_controller_manager control_msgs controller_manager_msgs + colcon test-result --verbose diff --git a/.github/workflows/humble-rhel-binary-build.yml b/.github/workflows/humble-rhel-binary-build.yml index bb00625455..ed37092520 100644 --- a/.github/workflows/humble-rhel-binary-build.yml +++ b/.github/workflows/humble-rhel-binary-build.yml @@ -1,9 +1,6 @@ -name: Humble RHEL Binary Build +name: RHEL Humble Binary Build on: workflow_dispatch: - push: - branches: - - humble pull_request: branches: - humble @@ -22,10 +19,13 @@ jobs: - uses: actions/checkout@v4 with: path: src/ros2_control - - run: | + - name: Install dependencies + run: | rosdep update rosdep install -iyr --from-path src/ros2_control || true + - name: Build and test + run: | source /opt/ros/${{ env.ROS_DISTRO }}/setup.bash colcon build --packages-skip rqt_controller_manager colcon test --packages-skip rqt_controller_manager ros2controlcli - colcon test-result + colcon test-result --verbose diff --git a/.github/workflows/iron-debian-build.yml b/.github/workflows/iron-debian-build.yml new file mode 100644 index 0000000000..405e4f9135 --- /dev/null +++ b/.github/workflows/iron-debian-build.yml @@ -0,0 +1,30 @@ +name: Debian Iron Build +on: + workflow_dispatch: + pull_request: + branches: + - iron + schedule: + # Run every day to detect flakiness and broken dependencies + - cron: '03 1 * * *' + + +jobs: + iron_debian: + name: Iron debian build + runs-on: ubuntu-latest + env: + ROS_DISTRO: iron + container: ghcr.io/ros-controls/ros:iron-debian + steps: + - uses: actions/checkout@v4 + with: + path: src/ros2_control + - name: Build and test + shell: bash + run: | + source /opt/ros2_ws/install/setup.bash + vcs import src < src/ros2_control/ros2_control.${{ env.ROS_DISTRO }}.repos + colcon build --packages-skip rqt_controller_manager + colcon test --packages-skip rqt_controller_manager control_msgs controller_manager_msgs + colcon test-result --verbose diff --git a/.github/workflows/iron-rhel-binary-build.yml b/.github/workflows/iron-rhel-binary-build.yml index 67d51b216c..fc48bd80ea 100644 --- a/.github/workflows/iron-rhel-binary-build.yml +++ b/.github/workflows/iron-rhel-binary-build.yml @@ -1,9 +1,6 @@ -name: Iron RHEL Binary Build +name: RHEL Iron Binary Build on: workflow_dispatch: - push: - branches: - - iron pull_request: branches: - iron @@ -23,10 +20,13 @@ jobs: - uses: actions/checkout@v4 with: path: src/ros2_control - - run: | + - name: Install dependencies + run: | rosdep update rosdep install -iyr --from-path src/ros2_control || true + - name: Build and test + run: | source /opt/ros/${{ env.ROS_DISTRO }}/setup.bash colcon build --packages-skip rqt_controller_manager colcon test --packages-skip rqt_controller_manager ros2controlcli - colcon test-result + colcon test-result --verbose diff --git a/.github/workflows/rolling-debian-build.yml b/.github/workflows/rolling-debian-build.yml new file mode 100644 index 0000000000..098af45029 --- /dev/null +++ b/.github/workflows/rolling-debian-build.yml @@ -0,0 +1,30 @@ +name: Debian Rolling Build +on: + workflow_dispatch: + pull_request: + branches: + - master + schedule: + # Run every day to detect flakiness and broken dependencies + - cron: '03 1 * * *' + + +jobs: + rolling_debian: + name: Rolling debian build + runs-on: ubuntu-latest + env: + ROS_DISTRO: rolling + container: ghcr.io/ros-controls/ros:rolling-debian + steps: + - uses: actions/checkout@v4 + with: + path: src/ros2_control + - name: Build and test + shell: bash + run: | + source /opt/ros2_ws/install/setup.bash + vcs import src < src/ros2_control/ros2_control.${{ env.ROS_DISTRO }}.repos + colcon build --packages-skip rqt_controller_manager + colcon test --packages-skip rqt_controller_manager + colcon test-result --verbose diff --git a/.github/workflows/rolling-rhel-binary-build.yml b/.github/workflows/rolling-rhel-binary-build.yml index 4e1a9f49d9..06a5411c24 100644 --- a/.github/workflows/rolling-rhel-binary-build.yml +++ b/.github/workflows/rolling-rhel-binary-build.yml @@ -1,9 +1,6 @@ -name: Rolling RHEL Binary Build +name: RHEL Rolling Binary Build on: workflow_dispatch: - push: - branches: - - master pull_request: branches: - master @@ -23,10 +20,13 @@ jobs: - uses: actions/checkout@v4 with: path: src/ros2_control - - run: | + - name: Install dependencies + run: | rosdep update rosdep install -iyr --from-path src/ros2_control || true + - name: Build and test + run: | source /opt/ros/${{ env.ROS_DISTRO }}/setup.bash colcon build --packages-skip rqt_controller_manager colcon test --packages-skip rqt_controller_manager ros2controlcli - colcon test-result + colcon test-result --verbose diff --git a/controller_manager/controller_manager/spawner.py b/controller_manager/controller_manager/spawner.py index 3ca5600997..cf78442856 100644 --- a/controller_manager/controller_manager/spawner.py +++ b/controller_manager/controller_manager/spawner.py @@ -19,8 +19,6 @@ import sys import time import warnings -import io -from contextlib import redirect_stdout, redirect_stderr from controller_manager import ( configure_controller, @@ -37,7 +35,6 @@ from rclpy.parameter import get_parameter_value from rclpy.signals import SignalHandlerOptions from ros2param.api import call_set_parameters -from ros2param.api import load_parameter_file # from https://stackoverflow.com/a/287944 @@ -266,6 +263,38 @@ def main(args=None): ) return 1 + if param_file: + parameter = Parameter() + parameter.name = prefixed_controller_name + ".params_file" + parameter.value = get_parameter_value(string_value=param_file) + + response = call_set_parameters( + node=node, node_name=controller_manager_name, parameters=[parameter] + ) + assert len(response.results) == 1 + result = response.results[0] + if result.successful: + node.get_logger().info( + bcolors.OKCYAN + + 'Set controller params file to "' + + param_file + + '" for ' + + bcolors.BOLD + + prefixed_controller_name + + bcolors.ENDC + ) + else: + node.get_logger().fatal( + bcolors.FAIL + + 'Could not set controller params file to "' + + param_file + + '" for ' + + bcolors.BOLD + + prefixed_controller_name + + bcolors.ENDC + ) + return 1 + ret = load_controller(node, controller_manager_name, controller_name) if not ret.ok: node.get_logger().fatal( @@ -284,39 +313,6 @@ def main(args=None): + bcolors.ENDC ) - if param_file: - # load_parameter_file writes to stdout/stderr. Here we capture that and use node logging instead - with redirect_stdout(io.StringIO()) as f_stdout, redirect_stderr( - io.StringIO() - ) as f_stderr: - load_parameter_file( - node=node, - node_name=prefixed_controller_name, - parameter_file=param_file, - use_wildcard=True, - ) - if f_stdout.getvalue(): - node.get_logger().info(bcolors.OKCYAN + f_stdout.getvalue() + bcolors.ENDC) - if f_stderr.getvalue(): - node.get_logger().error(bcolors.FAIL + f_stderr.getvalue() + bcolors.ENDC) - node.get_logger().info( - bcolors.OKCYAN - + 'Loaded parameters file "' - + param_file - + '" for ' - + bcolors.BOLD - + prefixed_controller_name - + bcolors.ENDC - ) - # TODO(destogl): use return value when upstream return value is merged - # ret = - # if ret.returncode != 0: - # Error message printed by ros2 param - # return ret.returncode - node.get_logger().info( - "Loaded " + param_file + " into " + prefixed_controller_name - ) - if not args.load_only: ret = configure_controller(node, controller_manager_name, controller_name) if not ret.ok: diff --git a/controller_manager/include/controller_manager/controller_manager.hpp b/controller_manager/include/controller_manager/controller_manager.hpp index 44b3c3215a..f5ee6854a1 100644 --- a/controller_manager/include/controller_manager/controller_manager.hpp +++ b/controller_manager/include/controller_manager/controller_manager.hpp @@ -414,6 +414,16 @@ class ControllerManager : public rclcpp::Node const std::vector & controllers); void controller_activity_diagnostic_callback(diagnostic_updater::DiagnosticStatusWrapper & stat); + + /** + * @brief determine_controller_node_options - A method that retrieves the controller defined node + * options and adapts them, based on if there is a params file to be loaded or the use_sim_time + * needs to be set + * @param controller - controller info + * @return The node options that will be set to the controller LifeCycleNode + */ + rclcpp::NodeOptions determine_controller_node_options(const ControllerSpec & controller) const; + diagnostic_updater::Updater diagnostics_updater_; std::shared_ptr executor_; diff --git a/controller_manager/src/controller_manager.cpp b/controller_manager/src/controller_manager.cpp index f588720b3d..ab57c5a196 100644 --- a/controller_manager/src/controller_manager.cpp +++ b/controller_manager/src/controller_manager.cpp @@ -574,6 +574,23 @@ controller_interface::ControllerInterfaceBaseSharedPtr ControllerManager::load_c controller_spec.next_update_cycle_time = std::make_shared( 0, 0, this->get_node_clock_interface()->get_clock()->get_clock_type()); + // We have to fetch the parameters_file at the time of loading the controller, because this way we + // can load them at the creation of the LifeCycleNode and this helps in using the features such as + // read_only params, dynamic maps lists etc + // Now check if the parameters_file parameter exist + const std::string param_name = controller_name + ".params_file"; + std::string parameters_file; + + // Check if parameter has been declared + if (!has_parameter(param_name)) + { + declare_parameter(param_name, rclcpp::ParameterType::PARAMETER_STRING); + } + if (get_parameter(param_name, parameters_file) && !parameters_file.empty()) + { + controller_spec.info.parameters_file = parameters_file; + } + return add_controller_impl(controller_spec); } @@ -1300,10 +1317,11 @@ controller_interface::ControllerInterfaceBaseSharedPtr ControllerManager::add_co return nullptr; } + const rclcpp::NodeOptions controller_node_options = determine_controller_node_options(controller); if ( controller.c->init( - controller.info.name, robot_description_, get_update_rate(), get_namespace()) == - controller_interface::return_type::ERROR) + controller.info.name, robot_description_, get_update_rate(), get_namespace(), + controller_node_options) == controller_interface::return_type::ERROR) { to.clear(); RCLCPP_ERROR( @@ -1311,17 +1329,6 @@ controller_interface::ControllerInterfaceBaseSharedPtr ControllerManager::add_co return nullptr; } - // ensure controller's `use_sim_time` parameter matches controller_manager's - const rclcpp::Parameter use_sim_time = this->get_parameter("use_sim_time"); - if (use_sim_time.as_bool()) - { - RCLCPP_INFO( - get_logger(), - "Setting use_sim_time=True for %s to match controller manager " - "(see ros2_control#325 for details)", - controller.info.name.c_str()); - controller.c->get_node()->set_parameter(use_sim_time); - } executor_->add_node(controller.c->get_node()->get_node_base_interface()); to.emplace_back(controller); @@ -2593,4 +2600,39 @@ void ControllerManager::controller_activity_diagnostic_callback( } } +rclcpp::NodeOptions ControllerManager::determine_controller_node_options( + const ControllerSpec & controller) const +{ + auto check_for_element = [](const auto & list, const auto & element) + { return std::find(list.begin(), list.end(), element) != list.end(); }; + + rclcpp::NodeOptions controller_node_options = rclcpp::NodeOptions().enable_logger_service(true); + std::vector node_options_arguments = controller_node_options.arguments(); + const std::string ros_args_arg = "--ros-args"; + if (controller.info.parameters_file.has_value()) + { + if (!check_for_element(node_options_arguments, ros_args_arg)) + { + node_options_arguments.push_back(ros_args_arg); + } + node_options_arguments.push_back("--params-file"); + node_options_arguments.push_back(controller.info.parameters_file.value()); + } + + // ensure controller's `use_sim_time` parameter matches controller_manager's + const rclcpp::Parameter use_sim_time = this->get_parameter("use_sim_time"); + if (use_sim_time.as_bool()) + { + if (!check_for_element(node_options_arguments, ros_args_arg)) + { + node_options_arguments.push_back(ros_args_arg); + } + node_options_arguments.push_back("-p"); + node_options_arguments.push_back("use_sim_time:=true"); + } + + controller_node_options = controller_node_options.arguments(node_options_arguments); + return controller_node_options; +} + } // namespace controller_manager diff --git a/hardware_interface/include/hardware_interface/controller_info.hpp b/hardware_interface/include/hardware_interface/controller_info.hpp index 149e0a2f90..61a51a134a 100644 --- a/hardware_interface/include/hardware_interface/controller_info.hpp +++ b/hardware_interface/include/hardware_interface/controller_info.hpp @@ -15,6 +15,7 @@ #ifndef HARDWARE_INTERFACE__CONTROLLER_INFO_HPP_ #define HARDWARE_INTERFACE__CONTROLLER_INFO_HPP_ +#include #include #include @@ -32,6 +33,9 @@ struct ControllerInfo /// Controller type. std::string type; + /// Controller param file + std::optional parameters_file; + /// List of claimed interfaces by the controller. std::vector claimed_interfaces; }; diff --git a/joint_limits/CMakeLists.txt b/joint_limits/CMakeLists.txt index f1177db2a9..f9f601fd31 100644 --- a/joint_limits/CMakeLists.txt +++ b/joint_limits/CMakeLists.txt @@ -11,6 +11,7 @@ set(THIS_PACKAGE_INCLUDE_DEPENDS rclcpp rclcpp_lifecycle trajectory_msgs + urdf ) find_package(ament_cmake REQUIRED) @@ -67,6 +68,9 @@ if(BUILD_TESTING) ament_add_gtest_executable(joint_limits_rosparam_test test/joint_limits_rosparam_test.cpp) target_link_libraries(joint_limits_rosparam_test joint_limits) + ament_add_gtest(joint_limits_urdf_test test/joint_limits_urdf_test.cpp) + target_link_libraries(joint_limits_urdf_test joint_limits) + add_launch_test(test/joint_limits_rosparam.launch.py) install( TARGETS joint_limits_rosparam_test diff --git a/joint_limits/include/joint_limits/joint_limits_urdf.hpp b/joint_limits/include/joint_limits/joint_limits_urdf.hpp new file mode 100644 index 0000000000..daa0d707d8 --- /dev/null +++ b/joint_limits/include/joint_limits/joint_limits_urdf.hpp @@ -0,0 +1,85 @@ +// Copyright 2024 PAL Robotics S.L. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +/// \author Adolfo Rodriguez Tsouroukdissian + +#ifndef JOINT_LIMITS_URDF_HPP +#define JOINT_LIMITS_URDF_HPP + +#include "joint_limits/joint_limits.hpp" +#include "urdf_model/joint.h" + +namespace joint_limits +{ + +/** + * \brief Populate a JointLimits instance from URDF joint data. + * \param[in] urdf_joint URDF joint. + * \param[out] limits Where URDF joint limit data gets written into. Limits in \e urdf_joint will + * overwrite existing values. Values in \e limits not present in \e urdf_joint remain unchanged. + * \return True if \e urdf_joint has a valid limits specification, false otherwise. + */ +inline bool getJointLimits(urdf::JointConstSharedPtr urdf_joint, JointLimits & limits) +{ + if (!urdf_joint || !urdf_joint->limits) + { + return false; + } + + limits.has_position_limits = + urdf_joint->type == urdf::Joint::REVOLUTE || urdf_joint->type == urdf::Joint::PRISMATIC; + if (limits.has_position_limits) + { + limits.min_position = urdf_joint->limits->lower; + limits.max_position = urdf_joint->limits->upper; + } + + if (!limits.has_position_limits && urdf_joint->type == urdf::Joint::CONTINUOUS) + { + limits.angle_wraparound = true; + } + + limits.has_velocity_limits = true; + limits.max_velocity = std::abs(urdf_joint->limits->velocity); + + limits.has_acceleration_limits = false; + + limits.has_effort_limits = true; + limits.max_effort = std::abs(urdf_joint->limits->effort); + + return true; +} + +/** + * \brief Populate a SoftJointLimits instance from URDF joint data. + * \param[in] urdf_joint URDF joint. + * \param[out] soft_limits Where URDF soft joint limit data gets written into. + * \return True if \e urdf_joint has a valid soft limits specification, false otherwise. + */ +inline bool getSoftJointLimits(urdf::JointConstSharedPtr urdf_joint, SoftJointLimits & soft_limits) +{ + if (!urdf_joint || !urdf_joint->safety) + { + return false; + } + + soft_limits.min_position = urdf_joint->safety->soft_lower_limit; + soft_limits.max_position = urdf_joint->safety->soft_upper_limit; + soft_limits.k_position = urdf_joint->safety->k_position; + soft_limits.k_velocity = urdf_joint->safety->k_velocity; + + return true; +} +} // namespace joint_limits +#endif // JOINT_LIMITS_URDF_HPP diff --git a/joint_limits/package.xml b/joint_limits/package.xml index 8c715e62aa..4e5e7097dd 100644 --- a/joint_limits/package.xml +++ b/joint_limits/package.xml @@ -20,6 +20,7 @@ rclcpp rclcpp_lifecycle trajectory_msgs + urdf ament_cmake_gmock ament_cmake_gtest diff --git a/joint_limits/test/joint_limits_urdf_test.cpp b/joint_limits/test/joint_limits_urdf_test.cpp new file mode 100644 index 0000000000..27d660afd7 --- /dev/null +++ b/joint_limits/test/joint_limits_urdf_test.cpp @@ -0,0 +1,177 @@ +// Copyright 2024 PAL Robotics S.L. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +/// \author Adolfo Rodriguez Tsouroukdissian +#include "joint_limits/joint_limits_urdf.hpp" +#include "gtest/gtest.h" + +using std::string; +using namespace joint_limits; + +class JointLimitsUrdfTest : public ::testing::Test +{ +public: + JointLimitsUrdfTest() + { + urdf_limits.reset(new urdf::JointLimits); + urdf_limits->effort = 8.0; + urdf_limits->velocity = 2.0; + urdf_limits->lower = -1.0; + urdf_limits->upper = 1.0; + + urdf_safety.reset(new urdf::JointSafety); + urdf_safety->k_position = 20.0; + urdf_safety->k_velocity = 40.0; + urdf_safety->soft_lower_limit = -0.8; + urdf_safety->soft_upper_limit = 0.8; + + urdf_joint.reset(new urdf::Joint); + urdf_joint->limits = urdf_limits; + urdf_joint->safety = urdf_safety; + + urdf_joint->type = urdf::Joint::UNKNOWN; + } + +protected: + urdf::JointLimitsSharedPtr urdf_limits; + urdf::JointSafetySharedPtr urdf_safety; + urdf::JointSharedPtr urdf_joint; +}; + +TEST_F(JointLimitsUrdfTest, GetJointLimits) +{ + // Unset URDF joint + { + JointLimits limits; + urdf::JointSharedPtr urdf_joint_bad; + EXPECT_FALSE(getJointLimits(urdf_joint_bad, limits)); + } + + // Unset URDF limits + { + JointLimits limits; + urdf::JointSharedPtr urdf_joint_bad(new urdf::Joint); + EXPECT_FALSE(getJointLimits(urdf_joint_bad, limits)); + } + + // Valid URDF joint, CONTINUOUS type + { + urdf_joint->type = urdf::Joint::CONTINUOUS; + + JointLimits limits; + EXPECT_TRUE(getJointLimits(urdf_joint, limits)); + + // Position + EXPECT_FALSE(limits.has_position_limits); + EXPECT_TRUE(limits.angle_wraparound); + + // Velocity + EXPECT_TRUE(limits.has_velocity_limits); + EXPECT_DOUBLE_EQ(urdf_joint->limits->velocity, limits.max_velocity); + + // Acceleration + EXPECT_FALSE(limits.has_acceleration_limits); + + // Effort + EXPECT_TRUE(limits.has_effort_limits); + EXPECT_DOUBLE_EQ(urdf_joint->limits->effort, limits.max_effort); + } + + // Valid URDF joint, REVOLUTE type + { + urdf_joint->type = urdf::Joint::REVOLUTE; + + JointLimits limits; + EXPECT_TRUE(getJointLimits(urdf_joint, limits)); + + // Position + EXPECT_TRUE(limits.has_position_limits); + EXPECT_DOUBLE_EQ(urdf_joint->limits->lower, limits.min_position); + EXPECT_DOUBLE_EQ(urdf_joint->limits->upper, limits.max_position); + EXPECT_FALSE(limits.angle_wraparound); + + // Velocity + EXPECT_TRUE(limits.has_velocity_limits); + EXPECT_DOUBLE_EQ(urdf_joint->limits->velocity, limits.max_velocity); + + // Acceleration + EXPECT_FALSE(limits.has_acceleration_limits); + + // Effort + EXPECT_TRUE(limits.has_effort_limits); + EXPECT_DOUBLE_EQ(urdf_joint->limits->effort, limits.max_effort); + } + + // Valid URDF joint, PRISMATIC type + { + urdf_joint->type = urdf::Joint::PRISMATIC; + + JointLimits limits; + EXPECT_TRUE(getJointLimits(urdf_joint, limits)); + + // Position + EXPECT_TRUE(limits.has_position_limits); + EXPECT_DOUBLE_EQ(urdf_joint->limits->lower, limits.min_position); + EXPECT_DOUBLE_EQ(urdf_joint->limits->upper, limits.max_position); + EXPECT_FALSE(limits.angle_wraparound); + + // Velocity + EXPECT_TRUE(limits.has_velocity_limits); + EXPECT_DOUBLE_EQ(urdf_joint->limits->velocity, limits.max_velocity); + + // Acceleration + EXPECT_FALSE(limits.has_acceleration_limits); + + // Effort + EXPECT_TRUE(limits.has_effort_limits); + EXPECT_DOUBLE_EQ(urdf_joint->limits->effort, limits.max_effort); + } +} + +TEST_F(JointLimitsUrdfTest, GetSoftJointLimits) +{ + using namespace joint_limits; + + // Unset URDF joint + { + SoftJointLimits soft_limits; + urdf::JointSharedPtr urdf_joint_bad; + EXPECT_FALSE(getSoftJointLimits(urdf_joint_bad, soft_limits)); + } + + // Unset URDF limits + { + SoftJointLimits soft_limits; + urdf::JointSharedPtr urdf_joint_bad(new urdf::Joint); + EXPECT_FALSE(getSoftJointLimits(urdf_joint_bad, soft_limits)); + } + + // Valid URDF joint + { + SoftJointLimits soft_limits; + EXPECT_TRUE(getSoftJointLimits(urdf_joint, soft_limits)); + + // Soft limits + EXPECT_DOUBLE_EQ(urdf_joint->safety->soft_lower_limit, soft_limits.min_position); + EXPECT_DOUBLE_EQ(urdf_joint->safety->soft_upper_limit, soft_limits.max_position); + EXPECT_DOUBLE_EQ(urdf_joint->safety->k_position, soft_limits.k_position); + EXPECT_DOUBLE_EQ(urdf_joint->safety->k_velocity, soft_limits.k_velocity); + } +} + +int main(int argc, char ** argv) +{ + testing::InitGoogleTest(&argc, argv); + return RUN_ALL_TESTS(); +}