Skip to content

fix maurob scenario bringup gripper client #18

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

Open
wants to merge 4 commits into
base: integrate_new_gripper
Choose a base branch
from
Open

fix maurob scenario bringup gripper client #18

Changes from all commits
Commits
Show all changes
4 commits
Select commit Hold shift + click to select a range
b4024cb
gripper acion in robot client
Feb 10, 2025
53fbc14
stubs to get scenario working
Feb 21, 2025
7c01c55
fix earlier stubs
Feb 23, 2025
7585aea
gripper gauge adjustment implementation
Feb 28, 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
140 changes: 83 additions & 57 deletions sr_moveit2_utils/sr_moveit2_utils/robot_client_node.py
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -38,15 +38,16 @@
from rclpy.executors import MultiThreadedExecutor
from rclpy.callback_groups import MutuallyExclusiveCallbackGroup
from rclpy.node import Node
from rclpy.action import ActionServer, GoalResponse, CancelResponse
from rclpy.action import ActionServer, ActionClient, GoalResponse, CancelResponse
from rclpy.action.server import ServerGoalHandle, GoalStatus

from sr_manipulation_interfaces.action import PlanMoveTo, Manip
from sr_manipulation_interfaces.msg import ManipType, PlanExecState
from control_msgs.action import IOGripperCommand, SetIOGripperConfig
from control_msgs.msg import DynamicInterfaceValues

from geometry_msgs.msg import PoseStamped, Vector3Stamped, Pose

from maurob_gripper.gripper_client import GripperClient
from maurob_components.move_client import MoveClient
from sr_moveit2_utils.scene_manager_client import SceneManagerClient
from sr_ros2_python_utils.visualization_publishers import VisualizatonPublisher
Expand Down Expand Up @@ -118,16 +119,18 @@ def __init__(self, name, default_velocity_scaling_factor=0.5):
)

self.scene_client = SceneManagerClient()
self.get_logger().info("Init GripperClient ...")
# Gripper handler
# self.gripper_client = GripperClient(
# # tf_prefix=self.tf_prefix,
# # tcp_link_name=self.chain_tip_link,
# driver_ns=self.gripper_driver_ns,
# sim=sim,
# # move_client=self.move_client,
# )
# self.gripper_client = GripperClient(node=self, sim=self.sim, move_client=self.move_client, svc_cbg=self.service_callback_group, sub_cbg=self.subpub_callback_group)
self.get_logger().info("Init Gripper Action Clients ...")

self.gripper_commander = ActionClient(
self, IOGripperCommand, "/gripper/gripper_action"
)

self.gripper_configure_commander = ActionClient(
self, SetIOGripperConfig, "/gripper/reconfigure_gripper_action"
)
self.gripper_states_subscriber = self.create_subscription(DynamicInterfaceValues, "/gripper/gripper_states", self._gripper_states_callback, 1)
self.sorted_gripper_states = [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0]

self.get_logger().info("Init action servers ...")

# action servers
Expand Down Expand Up @@ -164,6 +167,26 @@ def __init__(self, name, default_velocity_scaling_factor=0.5):
# self.jtc_cmd_publisher = self.create_publisher(JointTrajectory, "/position_trajectory_controller/joint_trajectory", 1)
# self.jtc_state_subscriber = self.create_subscription(JointTrajectoryControllerState, "/position_trajectory_controller/state", self._jtc_state_callback, 1)

def _gripper_states_callback(self, msg: DynamicInterfaceValues):

for i, interface in enumerate(msg.states.interface_names):
if (interface == "EL1008/Stichmass_125mm_BG03"):
self.sorted_gripper_states[0] = msg.states.values[i]
elif (interface == "EL1008/Stichmass_250mm_BG04"):
self.sorted_gripper_states[1] = msg.states.values[i]
elif (interface == "EL2008/Stichmass_125_WQG5"):
self.sorted_gripper_states[2] = msg.states.values[i]
elif (interface == "EL1008/Hohenabfrage_BG5"):
self.sorted_gripper_states[3] = msg.states.values[i]
elif (interface == "EL1008/Bauteilabfrage_BG06"):
self.sorted_gripper_states[4] = msg.states.values[i]
elif (interface == "EL2008/Stichmass_250_WQG6"):
self.sorted_gripper_states[5] = msg.states.values[i]
elif (interface == "EL1008/Greifer_Geschloschen_BG02"):
self.sorted_gripper_states[6] = msg.states.values[i]
elif (interface == "EL1008/Greifer_Geoeffnet_BG01"):
self.sorted_gripper_states[7] = msg.states.values[i]

def plan_move_to_goal_cb(self, goal: PlanMoveTo.Goal):
self.get_logger().debug("Received new PlanMoveTo goal...")

Expand Down Expand Up @@ -352,7 +375,7 @@ def compute_manip_pose(
) -> Pose:
pose_source_frame = deepcopy(source_pose)
if use_offset:
offset_transformed = self.tcp_transforms.to_from_tcp_vec3_conversion(
offset_transformed = self.tcp_transforms.transform_vector3stamped_to_target_frame(
offset_dir, pose_source_frame.header.frame_id
)
# add offset
Expand Down Expand Up @@ -608,20 +631,21 @@ def manip_execute_cb(self, goal_handle: ServerGoalHandle):
# TODO the gripper client, in addition to specific commands should also handle generic commands like a posture, which is then compatible to all moveit executors using a grasp posture
# apply on gripper
if (
not self.gripper_client.sensor_status[1]
and not self.gripper_client.sensor_status[3]
self.sorted_gripper_states[7]
):
self.get_logger().debug(" Close gripper.")
gripper_response = self.gripper_client.send_close_request()
if gripper_response is None:
self.get_logger().error("Failed to close the gripper")
result.state.exec_state = PlanExecState.EXEC_ERROR
result.state.exec_message = "Failed to close gripper"
goal_handle.abort()
break
goal_msg = IOGripperCommand.Goal()
goal_msg.open = False
self.gripper_commander.wait_for_server()
response = self.gripper_commander.send_goal(goal_msg)
# result_response is of type PlanMoveTo.Result()
if response.result.success:
self.get_logger().debug("Gripper command success!!")
else:
self.get_logger().warn(f"Gripper failed with state {response.result.message}")
else:
self.get_logger().warn(
f" Not closing gripper due to status of sensor {self.gripper_client.sensor_status}"
f" Not closing gripper due to status of sensor: {self.sorted_gripper_states}"
)
# additionally handle attach
if manip == ManipType.MANIP_GRASP:
Expand All @@ -645,20 +669,20 @@ def manip_execute_cb(self, goal_handle: ServerGoalHandle):
# prepare posture
# apply on gripper
if (
not self.gripper_client.sensor_status[0]
and not self.gripper_client.sensor_status[2]
not self.sorted_gripper_states[7]
):
self.get_logger().debug(" Open gripper.")
gripper_response = self.gripper_client.send_open_request()
if gripper_response is None:
self.get_logger().error("Failed to open the gripper")
result.state.exec_state = PlanExecState.EXEC_ERROR
result.state.exec_message = "Failed to open gripper"
goal_handle.abort()
break
goal_msg = IOGripperCommand.Goal()
goal_msg.open = True
self.gripper_commander.wait_for_server()
response = self.gripper_commander.send_goal(goal_msg)
if response.result.success:
self.get_logger().debug("Gripper command success!!")
else:
self.get_logger().warn(f"Gripper failed with state {response.result.message}")
else:
self.get_logger().warn(
f" Not opening gripper due to status of sensor {self.gripper_client.sensor_status}"
f" Not opening gripper due to status of sensor: {self.sorted_gripper_states}"
)
# additionally handle detach
if manip == ManipType.MANIP_RELEASE:
Expand Down Expand Up @@ -703,20 +727,21 @@ def manip_execute_cb(self, goal_handle: ServerGoalHandle):
if gauge_value > 0.125: # large
# TODO move the test of the gripper state to the change_gauge function so that RobotClient does not have to know about status
if (
self.gripper_client.sensor_status[4]
and not self.gripper_client.sensor_status[5]
self.sorted_gripper_states[0]
and not self.sorted_gripper_states[1]
):
ret = self.gripper_client.change_gauge(wide=True)
if ret.success is not True:
self.get_logger().error(
f"Cannot adjust gripper, with error {ret.error_msg}"
)
result.state.exec_state = PlanExecState.EXEC_ERROR
result.state.exec_message = ret.error_msg
break
self.get_logger().debug("Adjusting to wide gripper width.")
goal_msg = SetIOGripperConfig.Goal()
goal_msg.config_name = "stichmass_250"
self.gripper_configure_commander.wait_for_server()
response = self.gripper_configure_commander.send_goal(goal_msg)
if response.result.result:
self.get_logger().debug("Gripper configure command success!!")
else:
self.get_logger().warn(f"Gripper failed with state {response.result.status}")
elif (
not self.gripper_client.sensor_status[4]
and not self.gripper_client.sensor_status[5]
not self.sorted_gripper_states[0]
and not self.sorted_gripper_states[1]
):
self.get_logger().error("Cannot adjust gripper, unknown state")
result.state.exec_state = PlanExecState.EXEC_ERROR
Expand All @@ -727,20 +752,21 @@ def manip_execute_cb(self, goal_handle: ServerGoalHandle):

else: # small
if (
not self.gripper_client.sensor_status[4]
and self.gripper_client.sensor_status[5]
not self.sorted_gripper_states[0]
and self.sorted_gripper_states[1]
):
ret = self.gripper_client.change_gauge(wide=False)
if ret.success is not True:
self.get_logger().error(
f"Cannot adjust gripper, with error {ret.error_msg}"
)
result.state.exec_state = PlanExecState.EXEC_ERROR
result.state.exec_message = ret.error_msg
break
self.get_logger().debug("Adjusting to narrow gripper width.")
goal_msg = SetIOGripperConfig.Goal()
goal_msg.config_name = "stichmass_125"
self.gripper_configure_commander.wait_for_server()
response = self.gripper_configure_commander.send_goal(goal_msg)
if response.result.result:
self.get_logger().debug("Gripper configure command success!!")
else:
self.get_logger().warn(f"Gripper failed with state {response.result.status}")
elif (
not self.gripper_client.sensor_status[4]
and not self.gripper_client.sensor_status[5]
not self.sorted_gripper_states[0]
and not self.sorted_gripper_states[1]
):
self.get_logger().error("Cannot adjust gripper, unknown state")
result.state.exec_state = PlanExecState.EXEC_ERROR
Expand Down