115 lines
4.7 KiB
Python
115 lines
4.7 KiB
Python
|
#! /usr/bin/env python3
|
||
|
|
|
||
|
|
import yaml
|
||
|
|
import rclpy
|
||
|
|
from rclpy.node import Node
|
||
|
|
from rbs_utils_interfaces.msg import AssemblyConfig, NamedPose, RelativeNamedPose
|
||
|
|
from geometry_msgs.msg import Point
|
||
|
|
import os
|
||
|
|
|
||
|
|
|
||
|
|
class ConfigPublisherNode(Node):
|
||
|
|
|
||
|
|
def __init__(self):
|
||
|
|
super().__init__('config_publisher_node')
|
||
|
|
self.publisher_ = self.create_publisher(AssemblyConfig, '/assembly_config', 10)
|
||
|
|
timer_period = 1 # seconds
|
||
|
|
self.timer = self.create_timer(timer_period, self.timer_callback)
|
||
|
|
if 'RBS_ASSEMBLY_DIR' in os.environ:
|
||
|
|
assembly_dir = os.environ['RBS_ASSEMBLY_DIR']
|
||
|
|
else:
|
||
|
|
assembly_dir = '~/assembly/robossembler-stuff'
|
||
|
|
self.assembly_dir = os.path.expanduser(assembly_dir)
|
||
|
|
datafolder = "asp-example"
|
||
|
|
# Read data from YAML file
|
||
|
|
with open(f"{self.assembly_dir}/{datafolder}/rbs_db.yaml") as f:
|
||
|
|
self.data = yaml.safe_load(f)
|
||
|
|
|
||
|
|
def timer_callback(self):
|
||
|
|
|
||
|
|
# Create AssemblyConfig message
|
||
|
|
msg = AssemblyConfig()
|
||
|
|
# Fill in the data from the YAML file
|
||
|
|
msg.assets_db = self.data['assets_db']
|
||
|
|
|
||
|
|
# Populate workspace
|
||
|
|
for wp in self.data['workspace']:
|
||
|
|
point = Point()
|
||
|
|
point.x = wp["x"]
|
||
|
|
point.y = wp["y"]
|
||
|
|
point.z = wp["z"]
|
||
|
|
msg.workspace.append(point)
|
||
|
|
|
||
|
|
# Populate absolute_part
|
||
|
|
for part in self.data['absolute_part']:
|
||
|
|
named_pose = NamedPose()
|
||
|
|
named_pose.name = part['name']
|
||
|
|
pose = part['pose']
|
||
|
|
named_pose.pose.position.x = pose['position']['x']
|
||
|
|
named_pose.pose.position.y = pose['position']['y']
|
||
|
|
named_pose.pose.position.z = pose['position']['z']
|
||
|
|
named_pose.pose.orientation.x = pose['orientation']['x']
|
||
|
|
named_pose.pose.orientation.y = pose['orientation']['y']
|
||
|
|
named_pose.pose.orientation.z = pose['orientation']['z']
|
||
|
|
named_pose.pose.orientation.w = pose['orientation']['w']
|
||
|
|
msg.absolute_part.append(named_pose)
|
||
|
|
|
||
|
|
# Populate relative_part
|
||
|
|
for part in self.data['relative_part']:
|
||
|
|
relative_named_pose = RelativeNamedPose()
|
||
|
|
relative_named_pose.name = part['name']
|
||
|
|
relative_named_pose.relative_at = part['relative_at']
|
||
|
|
pose = part['pose']
|
||
|
|
relative_named_pose.pose.position.x = pose['position']['x']
|
||
|
|
relative_named_pose.pose.position.y = pose['position']['y']
|
||
|
|
relative_named_pose.pose.position.z = pose['position']['z']
|
||
|
|
relative_named_pose.pose.orientation.x = pose['orientation']['x']
|
||
|
|
relative_named_pose.pose.orientation.y = pose['orientation']['y']
|
||
|
|
relative_named_pose.pose.orientation.z = pose['orientation']['z']
|
||
|
|
relative_named_pose.pose.orientation.w = pose['orientation']['w']
|
||
|
|
msg.relative_part.append(relative_named_pose)
|
||
|
|
|
||
|
|
# Populate grasp_pose
|
||
|
|
for part in self.data['grasp_pose']:
|
||
|
|
relative_named_pose = RelativeNamedPose()
|
||
|
|
relative_named_pose.name = part['name']
|
||
|
|
relative_named_pose.relative_at = part['relative_at']
|
||
|
|
pose = part['pose']
|
||
|
|
relative_named_pose.pose.position.x = pose['position']['x']
|
||
|
|
relative_named_pose.pose.position.y = pose['position']['y']
|
||
|
|
relative_named_pose.pose.position.z = pose['position']['z']
|
||
|
|
relative_named_pose.pose.orientation.x = pose['orientation']['x']
|
||
|
|
relative_named_pose.pose.orientation.y = pose['orientation']['y']
|
||
|
|
relative_named_pose.pose.orientation.z = pose['orientation']['z']
|
||
|
|
relative_named_pose.pose.orientation.w = pose['orientation']['w']
|
||
|
|
msg.grasp_pose.append(relative_named_pose)
|
||
|
|
|
||
|
|
# Populate extra_poses
|
||
|
|
for part in self.data['extra_poses']:
|
||
|
|
named_pose = NamedPose()
|
||
|
|
named_pose.name = part['name']
|
||
|
|
pose = part['pose']
|
||
|
|
named_pose.pose.position.x = pose['position']['x']
|
||
|
|
named_pose.pose.position.y = pose['position']['y']
|
||
|
|
named_pose.pose.position.z = pose['position']['z']
|
||
|
|
named_pose.pose.orientation.x = pose['orientation']['x']
|
||
|
|
named_pose.pose.orientation.y = pose['orientation']['y']
|
||
|
|
named_pose.pose.orientation.z = pose['orientation']['z']
|
||
|
|
named_pose.pose.orientation.w = pose['orientation']['w']
|
||
|
|
msg.extra_poses.append(named_pose)
|
||
|
|
|
||
|
|
# Publish the message
|
||
|
|
self.publisher_.publish(msg)
|
||
|
|
|
||
|
|
|
||
|
|
def main(args=None):
|
||
|
|
rclpy.init(args=args)
|
||
|
|
config_publisher_node = ConfigPublisherNode()
|
||
|
|
rclpy.spin(config_publisher_node)
|
||
|
|
config_publisher_node.destroy_node()
|
||
|
|
rclpy.shutdown()
|
||
|
|
|
||
|
|
|
||
|
|
if __name__ == '__main__':
|
||
|
|
main()
|