Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Fix potential deadlock and wait until service is available #87

Merged
merged 2 commits into from
Mar 4, 2025
Merged

Fix potential deadlock and wait until service is available #87

Changes from all commits
Commits
Show all changes
2 commits
Select commit Hold shift + click to select a range
6cf741c
Update moveit2.py
belalhmedan90 Mar 3, 2025
437d425
Apply pre-commit suggestions
AndrejOrsula Mar 4, 2025
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
45 changes: 20 additions & 25 deletions pymoveit2/moveit2.py
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -529,7 +529,7 @@ def plan(
# 100ms sleep
rate = self._node.create_rate(10)
while not future.done():
rate.sleep()
rclpy.spin_once(self._node, timeout_sec=1.0)

return self.get_trajectory(
future,
Expand Down Expand Up @@ -634,22 +634,15 @@ def plan_async(
tolerance=tolerance_joint_position,
weight=weight_joint_position,
)

# Define starting state for the plan (default to the current state)
if start_joint_state is not None:
if isinstance(start_joint_state, JointState):
self.__move_action_goal.request.start_state.joint_state = (
start_joint_state
)
while start_joint_state is None:
self._node._logger.warn(message="Joint states are not available yet!")
if self.__joint_state is not None:
start_joint_state = self.__joint_state
break
else:
self.__move_action_goal.request.start_state.joint_state = (
init_joint_state(
joint_names=self.__joint_names,
joint_positions=start_joint_state,
)
)
elif self.joint_state is not None:
self.__move_action_goal.request.start_state.joint_state = self.joint_state
rclpy.spin_once(self._node, timeout_sec=1.0)
self._node._logger.info(message="Joint states are available now")

# Plan trajectory asynchronously by service call
if cartesian:
Expand Down Expand Up @@ -756,7 +749,7 @@ def wait_until_executed(self) -> bool:
return False

while self.__is_motion_requested or self.__is_executing:
self.__wait_until_executed_rate.sleep()
self.__wait_until_executed_rclpy.spin_once(self._node, timeout_sec=1.0)

return self.motion_suceeded

Expand Down Expand Up @@ -1202,7 +1195,7 @@ def compute_fk(
# 100ms sleep
rate = self._node.create_rate(10)
while not future.done():
rate.sleep()
rclpy.spin_once(self._node, timeout_sec=1.0)

return self.get_compute_fk_result(future, fk_link_names=fk_link_names)

Expand Down Expand Up @@ -1266,7 +1259,7 @@ def compute_fk_async(

stamp = self._node.get_clock().now().to_msg()
self.__compute_fk_req.header.stamp = stamp

self.__compute_fk_client.wait_for_service(timeout_sec=3.0)
if not self.__compute_fk_client.service_is_ready():
self._node.get_logger().warn(
f"Service '{self.__compute_fk_client.srv_name}' is not yet available. Better luck next time!"
Expand Down Expand Up @@ -1296,7 +1289,7 @@ def compute_ik(
# 10ms sleep
rate = self._node.create_rate(10)
while not future.done():
rate.sleep()
rclpy.spin_once(self._node, timeout_sec=1.0)

return self.get_compute_ik_result(future)

Expand Down Expand Up @@ -1391,7 +1384,7 @@ def compute_ik_async(

stamp = self._node.get_clock().now().to_msg()
self.__compute_ik_req.ik_request.pose_stamped.header.stamp = stamp

self.__compute_ik_client.wait_for_service(timeout_sec=3.0)
if not self.__compute_ik_client.wait_for_service(
timeout_sec=wait_for_server_timeout_sec
):
Expand Down Expand Up @@ -1828,7 +1821,7 @@ def update_planning_scene(self) -> bool:
Gets the current planning scene. Returns whether the service call was
successful.
"""

self._get_planning_scene_service.wait_for_service(timeout_sec=3.0)
if not self._get_planning_scene_service.service_is_ready():
self._node.get_logger().warn(
f"Service '{self._get_planning_scene_service.srv_name}' is not yet available. Better luck next time!"
Expand Down Expand Up @@ -1879,6 +1872,7 @@ def allow_collisions(self, id: str, allow: bool) -> Optional[Future]:
allowed_collision_matrix.entry_values[j] = allowed_collision_entry

# Apply the new planning scene
self._apply_planning_scene_service.wait_for_service(timeout_sec=3.0)
if not self._apply_planning_scene_service.service_is_ready():
self._node.get_logger().warn(
f"Service '{self._apply_planning_scene_service.srv_name}' is not yet available. Better luck next time!"
Expand Down Expand Up @@ -1923,6 +1917,7 @@ def clear_all_collision_objects(self) -> Optional[Future]:
self.__planning_scene.robot_state.attached_collision_objects = []

# Apply the new planning scene
self._apply_planning_scene_service.wait_for_service(timeout_sec=3.0)
if not self._apply_planning_scene_service.service_is_ready():
self._node.get_logger().warn(
f"Service '{self._apply_planning_scene_service.srv_name}' is not yet available. Better luck next time!"
Expand Down Expand Up @@ -1983,7 +1978,7 @@ def _plan_kinematic_path(self) -> Optional[Future]:
position_constraint.header.stamp = stamp
for orientation_constraint in constraints.orientation_constraints:
orientation_constraint.header.stamp = stamp

self._plan_kinematic_path_service.wait_for_service(timeout_sec=3.0)
if not self._plan_kinematic_path_service.service_is_ready():
self._node.get_logger().warn(
f"Service '{self._plan_kinematic_path_service.srv_name}' is not yet available. Better luck next time!"
Expand Down Expand Up @@ -2054,7 +2049,7 @@ def _plan_cartesian_path(
)

self.__cartesian_path_request.waypoints = [target_pose]

self._plan_cartesian_path_service.wait_for_service(timeout_sec=3.0)
if not self._plan_cartesian_path_service.service_is_ready():
self._node.get_logger().warn(
f"Service '{self._plan_cartesian_path_service.srv_name}' is not yet available. Better luck next time!"
Expand Down Expand Up @@ -2112,7 +2107,7 @@ def __result_callback_move_action(self, res):
self.__execution_mutex.acquire()
if res.result().status != GoalStatus.STATUS_SUCCEEDED:
self._node.get_logger().warn(
f"Action '{self.__move_action_client._action_name}' was unsuccessful: {enum_to_str(GoalStatus,res.result().status)}."
f"Action '{self.__move_action_client._action_name}' was unsuccessful: {enum_to_str(GoalStatus, res.result().status)}."
)
self.motion_suceeded = False
else:
Expand Down Expand Up @@ -2178,7 +2173,7 @@ def __result_callback_execute_trajectory(self, res):
self.__execution_mutex.acquire()
if res.result().status != GoalStatus.STATUS_SUCCEEDED:
self._node.get_logger().warn(
f"Action '{self._execute_trajectory_action_client._action_name}' was unsuccessful: {enum_to_str(GoalStatus,res.result().status)}."
f"Action '{self._execute_trajectory_action_client._action_name}' was unsuccessful: {enum_to_str(GoalStatus, res.result().status)}."
)
self.motion_suceeded = False
else:
Expand Down