Add a test to check if trajectory message is received

[christophfroehlich]

could be improved by checking if the robot actually moves

[christophfroehlich]

I had problems in my up-to-date rolling devcontainer to receive the topic, so I wrote a test for that. It seems that something changed in rclcpp/fastrtps on rolling making the send_trajectory node broken? Because it succeeds on jazzy here. @mikelikesrobots @sea-bass which environments did you use?

[sea-bass]

I used Jazzy with CycloneDDS on my end.

I almost never use FastDDS due to its reliability, and also due to the rename my binary install on my home PC actually makes my nodes crash if I use this middleware. (I've been too lazy to clean reinstall)

[mikelikesrobots]

I tried in Jazzy before @sea-bass 's changes, and it missed the occasional message. I even mention it in my video! I do have in my notes that I tried Zenoh as a middleware to see if that fixes it, and it was reliable with that. Given @sea-bass uses CycloneDDS, I'd say that's a clue.

[sea-bass]

Another thing to verify: Are there any QoS differences in the publisher/subscriber pair in this example's r6bot_controller vs. the standard ros2_controllers Joint trajectory controller?

JTC uses SystemDefaultsQos : https://github.com/ros-controls/ros2_controllers/blob/23b8ffc24bb74bf0f96679af73d5e9344b7d3809/joint_trajectory_controller/src/joint_trajectory_controller.cpp#L871

so does the r6bot_controller :

ros2_control_demos/example_7/controller/r6bot_controller.cpp

Line 94 in 1dc20c1

"~/joint_trajectory", rclcpp::SystemDefaultsQoS(), callback);

... however, the example publisher does not:

ros2_control_demos/example_7/reference_generator/send_trajectory.cpp

Lines 28 to 29 in 1dc20c1

	auto pub = node->create_publisher<trajectory_msgs::msg::JointTrajectory>(
	"/r6bot_controller/joint_trajectory", 10);

-- what if we change this to use SystemDefaultsQos?

[christophfroehlich]

yes there are, the send_trajectory node uses RELIABLE, while the controller BEST_EFFORT which should be compatible. Btw I can't even echo the topic via ros2 cli.

I tried now with cyclone on rolling, with
RMW_IMPLEMENTATION=rmw_cyclonedds_cpp colcon test --packages-select ros2_control_demo_example_7 the message is received but later I get 👀 @saikishor

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'].

I haven't rebuilt the stack, but that shouldn't be necessary?