Bill Finger
2156e9a0c9
- Adding new fixed joint links (`right_contact_panel` and `left_contact_panel`) to Link8 and Link7. - Defining visual representations with semi-transparent boxes for these panels. - Setting up collision models to enable accurate contact detection. - Updating Gazebo references to maintain fixed joint properties in simulations. - Moving sensors from the main links to the new contact panels for better interaction sensing. - Correcting and updating collision references in sensors. - Making minor XML formatting adjustments for consistency.
72 lines
2.4 KiB
Python
Executable file
72 lines
2.4 KiB
Python
Executable file
#!/usr/bin/env python3
|
|
|
|
import os
|
|
from typing import Dict
|
|
|
|
import rclpy
|
|
import yaml
|
|
from ament_index_python.packages import get_package_share_directory
|
|
from geometry_msgs.msg import Pose
|
|
from rbs_utils_interfaces.srv import GetNamedPose
|
|
from rclpy.node import Node
|
|
from rclpy.service import Service
|
|
|
|
|
|
class GetNamedPoseService(Node):
|
|
def __init__(self) -> None:
|
|
super().__init__("get_named_pose_service")
|
|
self.srv: Service = self.create_service(
|
|
GetNamedPose, "get_named_pose", self.get_named_pose
|
|
)
|
|
yaml_file_path: str = os.path.join(
|
|
get_package_share_directory("rbs_mill_assist"), "config", "key_poses.yaml"
|
|
)
|
|
with open(yaml_file_path, "r", encoding="utf-8") as file:
|
|
self.key_poses: Dict = yaml.safe_load(file)
|
|
|
|
def create_pose(self, pose_data: Dict) -> Pose:
|
|
"""
|
|
Helper function to create a Pose from the given pose data.
|
|
"""
|
|
pose = Pose()
|
|
if pose_data:
|
|
pose.position.x = pose_data.get("position", {}).get("x", 0.0)
|
|
pose.position.y = pose_data.get("position", {}).get("y", 0.0)
|
|
pose.position.z = pose_data.get("position", {}).get("z", 0.0)
|
|
pose.orientation.x = pose_data.get("orientation", {}).get("x", 0.0)
|
|
pose.orientation.y = pose_data.get("orientation", {}).get("y", 0.0)
|
|
pose.orientation.z = pose_data.get("orientation", {}).get("z", 0.0)
|
|
pose.orientation.w = pose_data.get("orientation", {}).get("w", 1.0)
|
|
return pose
|
|
|
|
def get_named_pose(
|
|
self, request: GetNamedPose.Request, response: GetNamedPose.Response
|
|
) -> GetNamedPose.Response:
|
|
|
|
key_pose = self.key_poses.get(request.pose_name, None)
|
|
if key_pose is None:
|
|
response.ok = False
|
|
return response
|
|
|
|
response.named_pose.name = request.pose_name
|
|
response.named_pose.pose = self.create_pose(key_pose)
|
|
response.ok = True
|
|
|
|
return response
|
|
|
|
|
|
def main():
|
|
rclpy.init()
|
|
executor = rclpy.executors.SingleThreadedExecutor()
|
|
# executor = rclpy.executors.MultiThreadedExecutor() # can't be used
|
|
i_node = GetNamedPoseService()
|
|
executor.add_node(i_node)
|
|
try:
|
|
executor.spin()
|
|
except (KeyboardInterrupt, rclpy.executors.ExternalShutdownException):
|
|
i_node.destroy_node()
|
|
rclpy.shutdown()
|
|
|
|
|
|
if __name__ == "__main__":
|
|
main()
|