Incorrect Implementations inside MoveGroupInterface for waiting response from server

### Description

Hello everyone,
Firstly, thanks for this great project.  I want to inform you about some bad practices in move_group_interface . There occur some functionality problem in  
the part of `plan` method of MoveGroupInterface
https://github.com/moveit/moveit2/blob/672e0ecd48f471fe05057f81e6233272a692a156/moveit_ros/planning_interface/move_group_interface/src/move_group_interface.cpp#L691-L710 ,

the part of `move` method of MoveGroupInterface
https://github.com/moveit/moveit2/blob/672e0ecd48f471fe05057f81e6233272a692a156/moveit_ros/planning_interface/move_group_interface/src/move_group_interface.cpp#L767-L782

and maybe this part of `execute` method of MoveGroupInterface
https://github.com/moveit/moveit2/blob/672e0ecd48f471fe05057f81e6233272a692a156/moveit_ros/planning_interface/move_group_interface/src/move_group_interface.cpp#L831-L850

I also want to make a critic about node spinning inside separate thread in move_group_interface. For users who don't know node starts to spin in separate thread when move group interface is created, this logic maybe can be problem. I think we need better implementation for this part as well.

https://github.com/moveit/moveit2/blob/672e0ecd48f471fe05057f81e6233272a692a156/moveit_ros/planning_interface/move_group_interface/src/move_group_interface.cpp#L117-L120

Problem is that there is a possibility that request is sent by client but is not received forever by server, especially move_group_server. This can bring about move_group_interface to get stuck inside while loop and plan method not to finish its own job.

In addition, these implementations normally work for rmw_fastrtps and rmw_cyclonedds, but get stuck with rmw_zenoh.

If you want, i can deal with it and can try to come up with better implementation.

### Your environment
* ROS Distro: Rolling
* OS Version: Ubuntu 22.04
* Source Build
* Branch : main
* RMW : rmw_zenoh

### Steps to reproduce
Firstly, run zenoh router (be sure that [rmw_zenoh_cpp](https://github.com/ros2/rmw_zenoh) is built at your workspace)

```shell
export RMW_IMPLEMENTATION=rmw_zenoh_cpp
ros2 run rmw_zenoh_cpp rmw_zenohd
```

Secondly, Follow the MoveIt tutorials(but use [this tutorial repository](https://github.com/CihatAltiparmak/moveit2_tutorials) i forked) and run demo launch of MoveIt2 using following command.

```shell
export RMW_IMPLEMENTATION=rmw_zenoh_cpp
ros2 run moveit2_tutorials perception_pipeline_demo.launch.py
```

Run this benchmark code from [this repository](https://github.com/CihatAltiparmak/moveit_middleware_benchmark/tree/fix/refactor_codebase) using below command

```shell
export RMW_IMPLEMENTATION=rmw_zenoh_cpp
ros2 run moveit_middleware_benchmark test_scenario_perception_pipeline
```

### Expected behaviour
plan method of MoveGroupInterface should not get stuck. If planning has timeout due to the fact that server doesn't receive request even though sending request from move_group_interface client is successfull, it is returned timeout error by `plan` method of move_group_interface.

### Actual behaviour
plan method gets stuck inside while loop.

### Backtrace or Console output

I have no console output and backtrace logs at the moment.


	auto goal_handle_future = move_action_client_->async_send_goal(goal, send_goal_opts);

	// wait until send_goal_opts.result_callback is called
	while (!done)
	{
	std::this_thread::sleep_for(std::chrono::milliseconds(1));
	}

	if (code != rclcpp_action::ResultCode::SUCCEEDED)
	{
	RCLCPP_ERROR_STREAM(logger_, "MoveGroupInterface::plan() failed or timeout reached");
	return res->error_code;
	}

	plan.trajectory = res->planned_trajectory;
	plan.start_state = res->trajectory_start;
	plan.planning_time = res->planning_time;
	RCLCPP_INFO(logger_, "time taken to generate plan: %g seconds", plan.planning_time);

	return res->error_code;

	auto goal_handle_future = move_action_client_->async_send_goal(goal, send_goal_opts);
	if (!wait)
	return moveit::core::MoveItErrorCode::SUCCESS;

	// wait until send_goal_opts.result_callback is called
	while (!done)
	{
	std::this_thread::sleep_for(std::chrono::milliseconds(1));
	}

	if (code != rclcpp_action::ResultCode::SUCCEEDED)
	{
	RCLCPP_ERROR_STREAM(logger_, "MoveGroupInterface::move() failed or timeout reached");
	}
	return res->error_code;
	}

	moveit_msgs::action::ExecuteTrajectory::Goal goal;
	goal.trajectory = trajectory;
	goal.controller_names = controllers;

	auto goal_handle_future = execute_action_client_->async_send_goal(goal, send_goal_opts);
	if (!wait)
	return moveit::core::MoveItErrorCode::SUCCESS;

	// wait until send_goal_opts.result_callback is called
	while (!done)
	{
	std::this_thread::sleep_for(std::chrono::milliseconds(1));
	}

	if (code != rclcpp_action::ResultCode::SUCCEEDED)
	{
	RCLCPP_ERROR_STREAM(logger_, "MoveGroupInterface::execute() failed or timeout reached");
	}
	return res->error_code;
	}

	callback_group_ = node_->create_callback_group(rclcpp::CallbackGroupType::MutuallyExclusive,
	false /* don't spin with node executor */);
	callback_executor_.add_callback_group(callback_group_, node->get_node_base_interface());
	callback_thread_ = std::thread([this]() { callback_executor_.spin(); });

Provide feedback

Saved searches

Use saved searches to filter your results more quickly

Incorrect Implementations inside MoveGroupInterface for waiting response from server #2859

Description

Your environment

Steps to reproduce

Expected behaviour

Actual behaviour

Backtrace or Console output

Metadata

Assignees

Labels

Type

Projects

Milestone

Relationships

Development

Incorrect Implementations inside MoveGroupInterface for waiting response from server #2859

Description

Description

Your environment

Steps to reproduce

Expected behaviour

Actual behaviour

Backtrace or Console output

Metadata

Metadata

Assignees

Labels

Type

Projects

Milestone

Relationships

Development

Issue actions