Open
Description
Describe the bug
3: [ros2_control_node-1] terminate called after throwing an instance of 'std::bad_optional_access'
3: [ros2_control_node-1] what(): bad optional access
3: [ros2_control_node-1] Stack trace (most recent call last) in thread 71837:
3: [ros2_control_node-1] #24 Object "", at 0xffffffffffffffff, in
3: [ros2_control_node-1] #23 Object "/usr/lib/x86_64-linux-gnu/libc.so.6", at 0x7f071fd07c3b, in
3: [ros2_control_node-1] #22 Object "/usr/lib/x86_64-linux-gnu/libc.so.6", at 0x7f071fc7aaa3, in
3: [ros2_control_node-1] #21 Object "/usr/lib/x86_64-linux-gnu/libstdc++.so.6.0.33", at 0x7f071fff3db3, in
3: [ros2_control_node-1] #20 Source "/usr/include/c++/13/bits/std_thread.h", line 244, in _M_run [0x5c06125a4829]
3: [ros2_control_node-1] 241: { }
3: [ros2_control_node-1] 242:
3: [ros2_control_node-1] 243: void
3: [ros2_control_node-1] > 244: _M_run() { _M_func(); }
3: [ros2_control_node-1] 245: };
3: [ros2_control_node-1] 246:
3: [ros2_control_node-1] 247: void
3: [ros2_control_node-1] #19 Source "/usr/include/c++/13/bits/std_thread.h", line 299, in operator() [0x5c06125a4845]
3: [ros2_control_node-1] 296: {
3: [ros2_control_node-1] 297: using _Indices
3: [ros2_control_node-1] 298: = typename _Build_index_tuple<tuple_size<_Tuple>::value>::__type;
3: [ros2_control_node-1] > 299: return _M_invoke(_Indices());
3: [ros2_control_node-1] 300: }
3: [ros2_control_node-1] 301: };
3: [ros2_control_node-1] #18 Source "/usr/include/c++/13/bits/std_thread.h", line 292, in _M_invoke<0> [0x5c06125a4871]
3: [ros2_control_node-1] 289: template<size_t... _Ind>
3: [ros2_control_node-1] 290: typename __result<_Tuple>::type
3: [ros2_control_node-1] 291: _M_invoke(_Index_tuple<_Ind...>)
3: [ros2_control_node-1] > 292: { return std::__invoke(std::get<_Ind>(std::move(_M_t))...); }
3: [ros2_control_node-1] 293:
3: [ros2_control_node-1] 294: typename __result<_Tuple>::type
3: [ros2_control_node-1] 295: operator()()
3: [ros2_control_node-1] #17 Source "/usr/include/c++/13/bits/invoke.h", line 96, in __invoke<main(int, char**)::<lambda()> > [0x5c06125a48c4]
3: [ros2_control_node-1] 93: using __result = __invoke_result<_Callable, _Args...>;
3: [ros2_control_node-1] 94: using __type = typename __result::type;
3: [ros2_control_node-1] 95: using __tag = typename __result::__invoke_type;
3: [ros2_control_node-1] > 96: return std::__invoke_impl<__type>(__tag{}, std::forward<_Callable>(__fn),
3: [ros2_control_node-1] 97: std::forward<_Args>(__args)...);
3: [ros2_control_node-1] 98: }
3: [ros2_control_node-1] #16 Source "/usr/include/c++/13/bits/invoke.h", line 61, in __invoke_impl<void, main(int, char**)::<lambda()> > [0x5c06125a4901]
3: [ros2_control_node-1] 58: template<typename _Res, typename _Fn, typename... _Args>
3: [ros2_control_node-1] 59: constexpr _Res
3: [ros2_control_node-1] 60: __invoke_impl(__invoke_other, _Fn&& __f, _Args&&... __args)
3: [ros2_control_node-1] > 61: { return std::forward<_Fn>(__f)(std::forward<_Args>(__args)...); }
3: [ros2_control_node-1] 62:
3: [ros2_control_node-1] 63: template<typename _Res, typename _MemFun, typename _Tp, typename... _Args>
3: [ros2_control_node-1] 64: constexpr _Res
3: [ros2_control_node-1] #15 Source "/workspaces/ros2_rolling_ws/src/ros2_control/controller_manager/src/ros2_control_node.cpp", line 143, in operator() [0x5c06125a2345]
3: [ros2_control_node-1] 141: // execute update loop
3: [ros2_control_node-1] 142: cm->read(cm->get_trigger_clock()->now(), measured_period);
3: [ros2_control_node-1] > 143: cm->update(cm->get_trigger_clock()->now(), measured_period);
3: [ros2_control_node-1] 144: cm->write(cm->get_trigger_clock()->now(), measured_period);
3: [ros2_control_node-1] 145:
3: [ros2_control_node-1] 146: // wait until we hit the end of the period
3: [ros2_control_node-1] #14 Source "/workspaces/ros2_rolling_ws/src/ros2_control/controller_manager/src/controller_manager.cpp", line 2877, in update [0x7f0720af7b9a]
3: [ros2_control_node-1] 2874: // To publish the activity of the failing controllers and the fallback controllers
3: [ros2_control_node-1] 2875: publish_activity();
3: [ros2_control_node-1] 2876: }
3: [ros2_control_node-1] >2877: resource_manager_->enforce_command_limits(period);
3: [ros2_control_node-1] 2878:
3: [ros2_control_node-1] 2879: // there are controllers to (de)activate
3: [ros2_control_node-1] 2880: if (switch_params_.do_switch)
3: [ros2_control_node-1] #13 Source "/workspaces/ros2_rolling_ws/src/ros2_control/hardware_interface/src/resource_manager.cpp", line 2128, in enforce_command_limits [0x7f071f86092e]
3: [ros2_control_node-1] 2125: {
3: [ros2_control_node-1] 2126: for (const auto & [joint_name, limiter] : limiters)
3: [ros2_control_node-1] 2127: {
3: [ros2_control_node-1] >2128: enforce_result |= resource_storage_->enforce_command_limits(joint_name, period);
3: [ros2_control_node-1] 2129: }
3: [ros2_control_node-1] 2130: }
3: [ros2_control_node-1] 2131: return enforce_result;
3: [ros2_control_node-1] #12 Source "/workspaces/ros2_rolling_ws/src/ros2_control/hardware_interface/src/resource_manager.cpp", line 819, in enforce_command_limits [0x7f071f8779c4]
3: [ros2_control_node-1] 816: {
3: [ros2_control_node-1] 817: joint_limits::JointInterfacesCommandLimiterData & data = limiters_data_[joint_name];
3: [ros2_control_node-1] 818: update_joint_limiters_data(data);
3: [ros2_control_node-1] > 819: enforce_result = limiters[joint_name]->enforce(data.actual, data.limited, period);
3: [ros2_control_node-1] 820: if (enforce_result)
3: [ros2_control_node-1] 821: {
3: [ros2_control_node-1] 822: update_joint_limiters_commands(data.limited, command_interface_map_);
3: [ros2_control_node-1] #11 Source "/workspaces/ros2_rolling_ws/src/ros2_control/joint_limits/include/joint_limits/joint_limiter_interface.hpp", line 208, in enforce [0x7f071f88199f]
3: [ros2_control_node-1] 205: JointLimitsStateDataType & desired_joint_states, const rclcpp::Duration & dt)
3: [ros2_control_node-1] 206: {
3: [ros2_control_node-1] 207: joint_limits_ = *(updated_limits_.readFromRT());
3: [ros2_control_node-1] > 208: return on_enforce(current_joint_states, desired_joint_states, dt);
3: [ros2_control_node-1] 209: }
3: [ros2_control_node-1] 210:
3: [ros2_control_node-1] 211: virtual void reset_internals() = 0;
3: [ros2_control_node-1] #10 Source "/workspaces/ros2_rolling_ws/src/ros2_control/joint_limits/src/joint_range_limiter.cpp", line 116, in on_enforce [0x7f071f2f1f45]
3: [ros2_control_node-1] 114: if (desired.has_position())
3: [ros2_control_node-1] 115: {
3: [ros2_control_node-1] > 116: const auto limits = compute_position_limits(
3: [ros2_control_node-1] 117: joint_name, joint_limits, actual.velocity, actual.position, prev_command_.position,
3: [ros2_control_node-1] 118: dt_seconds);
3: [ros2_control_node-1] 119: limits_enforced = is_limited(desired.position.value(), limits.lower_limit, limits.upper_limit);
3: [ros2_control_node-1] #9 Source "/workspaces/ros2_rolling_ws/src/ros2_control/joint_limits/src/joint_limits_helpers.cpp", line 93, in compute_position_limits [0x7f071f0642b4]
3: [ros2_control_node-1] 90: /// between the command sent to the robot and the robot actually showing that in the state. That
3: [ros2_control_node-1] 91: /// effectively limits the velocity with which the joint can be moved which is much lower than
3: [ros2_control_node-1] 92: /// the actual velocity limit.
3: [ros2_control_node-1] > 93: const double position_reference = prev_command_pos.value();
3: [ros2_control_node-1] 94: pos_limits.lower_limit = std::max(
3: [ros2_control_node-1] 95: std::min(position_reference - delta_pos, pos_limits.upper_limit), pos_limits.lower_limit);
3: [ros2_control_node-1] 96: pos_limits.upper_limit = std::min(
3: [ros2_control_node-1] #8 Source "/usr/include/c++/13/optional", line 997, in value [0x7f071f87c74e]
3: [ros2_control_node-1] 994: {
3: [ros2_control_node-1] 995: if (this->_M_is_engaged())
3: [ros2_control_node-1] 996: return this->_M_get();
3: [ros2_control_node-1] > 997: __throw_bad_optional_access();
3: [ros2_control_node-1] 998: }
3: [ros2_control_node-1] 999:
3: [ros2_control_node-1] 1000: constexpr _Tp&
3: [ros2_control_node-1] #7 Source "/usr/include/c++/13/optional", line 111, in __throw_bad_optional_access [0x7f0720b0b47d]
3: [ros2_control_node-1] 108: // XXX Does not belong here.
3: [ros2_control_node-1] 109: [[__noreturn__]] inline void
3: [ros2_control_node-1] 110: __throw_bad_optional_access()
3: [ros2_control_node-1] > 111: { _GLIBCXX_THROW_OR_ABORT(bad_optional_access()); }
3: [ros2_control_node-1] 112:
3: [ros2_control_node-1] 113: // This class template manages construction/destruction of
3: [ros2_control_node-1] 114: // the contained value for a std::optional.
3: [ros2_control_node-1] #6 Object "/usr/lib/x86_64-linux-gnu/libstdc++.so.6.0.33", at 0x7f071ffc2390, in __cxa_throw
3: [ros2_control_node-1] #5 Object "/usr/lib/x86_64-linux-gnu/libstdc++.so.6.0.33", at 0x7f071ffaca54, in std::terminate()
3: [ros2_control_node-1] #4 Object "/usr/lib/x86_64-linux-gnu/libstdc++.so.6.0.33", at 0x7f071ffc20d9, in
3: [ros2_control_node-1] #3 Object "/usr/lib/x86_64-linux-gnu/libstdc++.so.6.0.33", at 0x7f071ffacff4, in
3: [ros2_control_node-1] #2 Object "/usr/lib/x86_64-linux-gnu/libc.so.6", at 0x7f071fc068fe, in abort
3: [ros2_control_node-1] #1 Object "/usr/lib/x86_64-linux-gnu/libc.so.6", at 0x7f071fc2327d, in raise
3: [ros2_control_node-1] #0 Object "/usr/lib/x86_64-linux-gnu/libc.so.6", at 0x7f071fc7cb2c, in pthread_kill
3: [ros2_control_node-1] Aborted (Signal sent by tkill() 71795 1001)
3: [ERROR] [ros2_control_node-1]: process has died [pid 71795, exit code -6, cmd '/workspaces/ros2_rolling_ws/install/controller_manager/lib/controller_manager/ros2_control_node --ros-args --params-file /workspaces/ros2_rolling_ws/install/ros2_control_demo_example_7/share/ros2_control_demo_example_7/config/r6bot_controller.yaml'].
To Reproduce
Steps to reproduce the behavior:
- Compile the test added with Add a test to check if trajectory message is received ros2_control_demos#805
- Run
RMW_IMPLEMENTATION=rmw_cyclonedds_cpp colcon test --packages-select ros2_control_demo_example_7more often, because not everytime the message is received
Expected behavior
No throw
Environment (please complete the following information):
- OS: Ubuntu noble
- Version rolling from source
- rclcpp 29.4.0
- rmw_cyclonedds_cpp 4.0.1