I've created my own move_group_interface due to some complications with the original and the need to expose more functionality of move_group to my application. Within the Gazebo simulation it is working as intended, unfortunately it doesn't work on the real robot yet. I receive the error:
[motion_interface-14] [INFO][1708423092.297431201] [moveit_ros.current_state_monitor]: Didn't receive robot state (joint angles) with recent timestamp within 1.000000 seconds. Requested time 1708423091.297018, but last received state has time 1708423091.294824.
This is when I request the current robot state for the planning request to move_group the function bool CurrentStateMonitor::waitForCurrentState(const rclcpp::Time& t, double wait_time_s) const is called. When running without using simulation time, it will wait for a new message on /joint_states topic until the wait time is reached. It should be notified by void CurrentStateMonitor::jointStateCallback(const sensor_msgs::msg::JointState::ConstSharedPtr& joint_state). This function will get called until I request the current robot state, then it will be only called after the first function finished. For me it seems to be some kind of parallel execution issue, so I've tried calling waitForCurrentState by:
std::future<bool> hasCurrentState = std::async(&planning_scene_monitor::CurrentStateMonitor::waitForCurrentState,
this->options_.current_state_monitor,
this->node_ptr->get_clocke()->now(),
wait_seconds);
Sadly this hasn't yielded any positive result. Since the subscription to joint_states is convoluted and buried in CurrentStateMonitorMiddlewareHandle I have not really a good overview on what is happening. I presume some kind of need for multi-threaded executors or/and callback groups. I'm however hesitant to do changes in these parts of MoveIt. I've tried out to give the subscription a reentrant callback group, but also without positive result.
Maybe you can bring me on track of what is going on.
