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.