Update robot state if time since last command exceeds timeout

[KazuyaOguma18]

Description

When starting both move_group and moveit_servo simultaneously, controlling the arm with move_group first and then performing jog operations with moveit_servo resulted in the jog operations being executed from the arm's initial pose, without considering the results of the previous control.

To address this issue, the robot's state is now updated synchronously, halting other processes if no control commands are received for a duration exceeding the incoming_command_timeout, which indicates that no jog operations are being executed.

Checklist

• Required by CI: Code is auto formatted using clang-format
• Extend the tutorials / documentation reference
• Document API changes relevant to the user in the MIGRATION.md notes
• Create tests, which fail without this PR reference
• Include a screenshot if changing a GUI
• While waiting for someone to review your request, please help review another open pull request to support the maintainers

[sea-bass]

Thank you for this!

I'm wondering, how is this logic different from what is already in e.g.

moveit2/moveit_ros/moveit_servo/src/servo_node.cpp

Lines 213 to 229 in 2b9173b

	const bool command_stale = (node_->now() - latest_joint_jog_.header.stamp) >=
	rclcpp::Duration::from_seconds(servo_params_.incoming_command_timeout);
	if (!command_stale)
	{
	JointJogCommand command{ latest_joint_jog_.joint_names, latest_joint_jog_.velocities };
	next_joint_state = servo_->getNextJointState(robot_state, command);
	}
	else
	{
	auto result = servo_->smoothHalt(last_commanded_state_);
	new_joint_jog_msg_ = !result.first;
	if (new_joint_jog_msg_)
	{
	next_joint_state = result.second;
	RCLCPP_DEBUG_STREAM(node_->get_logger(), "Joint jog command timed out. Halting to a stop.");
	}
	}

?

Would you be able to instead fix the existing logic instead of adding new lines of code?

[KazuyaOguma18]

@sea-bass
Thank you for reviewing!

Would you be able to instead fix the existing logic instead of adding new lines of code?

Yes, I can. Is it correct to write the added code when command_stale in jog, twist and pose fuction is true?

[sea-bass]

Yes, I can. Is it correct to write the added code when command_stale in jog, twist and pose fuction is true?

Exactly! It would also be appreciated if you could write a unit test for this logic, to verify the fixes.

[sea-bass]

Looking at these changes, I now wonder why it is not sufficient to update last_commanded_state_ in here at the end of each iteration?

moveit2/moveit_ros/moveit_servo/src/servo_node.cpp

Line 399 in f821ad7

last_commanded_state_ = next_joint_state.value();

This logic is quite difficult to follow and full of hidden state, so I am likely missing something.

I ask because maybe there is a better place to update this variable in one place instead of doing it repetitively and multiple times in a single loop iteration of the servo node?

[sea-bass]

Ah, I think I see the issue now!

When I last ran into this case back when I used MoveIt, it was because I was pausing servo, using move_group to command the robot, and then resuming servo before sending new commands. In this case, we had fixed the logic by updating last_commanded_state_ upon unpausing.

If you do the same without pausing and unpausing servo, as it seems you are doing, then returning from a state of stale commands to new commands will indeed use old state.

I still think there are better ways to resolve this, maybe in the main servo loop instead of the individual processXYZ() functions. Maybe keeping around something that tracks whether the previous command was stale? What you would want is something like this:

if (previous command was stale and new one is not)
{
    last_commanded_state_ = current state;
}

One way I can think of doing this is by making last_commanded_state_ a std::optional, and when the servo commands go stale you set it to std::nullopt. So then this logic becomes the following at each loop iteration:

if (!last_commanded_state_)
{
    last_commanded_state_ = current state;
}

What do you think?

[sea-bass]

Follow up as I was reading the code on my computer instead of my phone...

Does the same issue get resolved if you update last_commanded_state_ to the current state in this block of code?

moveit2/moveit_ros/moveit_servo/src/servo_node.cpp

Lines 348 to 351 in 2302268

	// if all joint_cmd_rolling_window_ is empty or all commands in it are outdated, use current robot state
	joint_cmd_rolling_window_.clear();
	current_state = servo_->getCurrentRobotState(false /* block for current robot state */);
	current_state.velocities *= 0.0;

[KazuyaOguma18]

I still think there are better ways to resolve this, maybe in the main servo loop instead of the individual processXYZ() functions. Maybe keeping around something that tracks whether the previous command was stale? What you would want is something like this:

I also felt that updating last_command_state_ in many places would cause bugs, so I agree with using std::optional to determine valid and invalid values.
However, I need to add a way to deal with the case of nullopt in the input to functions that use last_command_state_.

Does the same issue get resolved if you update last_commanded_state_ to the current state in this block of code?

Indeed, this line is similar to the current problem of not updating the robot.
What is curious is that this line is not updated with blocking.

[sea-bass]

Does the same issue get resolved if you update last_commanded_state_ to the current state in this block of code?

Indeed, this line is similar to the current problem of not updating the robot. What is curious is that this line is not updated with blocking.

Right. This is to prevent the servo loop from taking too long to compute, whereas on pause/unpause this happens separately.

Even with blocking set to false, could you try whether this still works fine for your use case?

If you want to guarantee an up-to-date current state, my recommendation would be call the "pause_servo" service, then motion plan with move_group, and finally unpause servo with the same service.

[KazuyaOguma18]

Does the same issue get resolved if you update last_commanded_state_ to the current state in this block of code?

I checked this, but even when using current_state = servo_->getCurrentRobotState(true /* block for current robot state */);, the robot's state was not updated to the latest.

When I checked the condition expression, the size of joint_cmd_rolling_window_ was around 20.

Upon further review, I found that the value of next_joint_state being nullopt indicates that the robot is in a stopped command state and that no next target pose exists. So, I tried updating the robot's state in a blocking manner under these conditions. What do you think?

[sea-bass]

Upon further review, I found that the value of next_joint_state being nullopt indicates that the robot is in a stopped command state and that no next target pose exists. So, I tried updating the robot's state in a blocking manner under these conditions. What do you think?

Thank you for checking!

I am worried that this blocking call will mean the overall servo loop might miss its frequency (which is not desired especially for real-time applications) -- so I think it will be good to avoid blocking during the main loop.

However, it seems the place you have now added the code already has access to a current_state variable.

So does this happen to work for your problem instead?

if (!next_joint_state) {
  last_commanded_state_ = current_state_;
}

[KazuyaOguma18]

Not using blocking processing does not mean that execution cannot be performed using the robot's latest state. I found that the issue was actually caused by joint_cmd_rolling_window_ not containing the current posture at all.

Therefore, if a valid next_joint_state does not exist, I modified it to retrieve the latest robot state in a non-blocking manner using last_commanded_state_ and current_state.

[sea-bass]

Wonderful! Thank you for iterating with me on this, @KazuyaOguma18. I think the changes now are very simple, and if they solve your original issue, I am happy to approve.

[codecov-commenter]

⚠️ Please install the to ensure uploads and comments are reliably processed by Codecov.

Codecov Report

All modified and coverable lines are covered by tests ✅

Project coverage is 45.97%. Comparing base (75fb28b) to head (babb78a).
Report is 8 commits behind head on main.

❗ Your organization needs to install the Codecov GitHub app to enable full functionality.

Additional details and impacted files

@@            Coverage Diff             @@
##             main    #3251      +/-   ##
==========================================
+ Coverage   45.97%   45.97%   +0.01%     
==========================================
  Files         716      716              
  Lines       62403    62420      +17     
  Branches     7547     7551       +4     
==========================================
+ Hits        28681    28691      +10     
- Misses      33555    33562       +7     
  Partials      167      167              

☔ View full report in Codecov by Sentry.
📢 Have feedback on the report? Share it here.