asssembly config simple publisher
This commit is contained in:
parent
e8ea09e020
commit
ae48c5ee6a
4 changed files with 333 additions and 0 deletions
114
env_manager/gz_enviroment/scripts/publish_asm_config.py
Executable file
114
env_manager/gz_enviroment/scripts/publish_asm_config.py
Executable file
|
|
@ -0,0 +1,114 @@
|
|||
#! /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()
|
||||
75
env_manager/gz_enviroment/scripts/test_tf.py
Executable file
75
env_manager/gz_enviroment/scripts/test_tf.py
Executable file
|
|
@ -0,0 +1,75 @@
|
|||
#! /usr/bin/env python3
|
||||
|
||||
import rclpy
|
||||
from rclpy.node import Node
|
||||
from tf2_msgs.msg import TFMessage
|
||||
from geometry_msgs.msg import TransformStamped
|
||||
from rbs_utils_interfaces.msg import AssemblyConfig
|
||||
from tf2_ros.transform_broadcaster import TransformBroadcaster
|
||||
|
||||
|
||||
class TF2PublisherNode(Node):
|
||||
|
||||
def __init__(self):
|
||||
super().__init__('tf2_publisher_node')
|
||||
self.tfb_ = TransformBroadcaster(self)
|
||||
self.subscription = self.create_subscription(
|
||||
AssemblyConfig, '/assembly_config', self.config_callback, 10)
|
||||
|
||||
def config_callback(self, msg):
|
||||
# Populate transforms for absolute_part
|
||||
for part in msg.absolute_part:
|
||||
ts = TransformStamped()
|
||||
ts.header.stamp = self.get_clock().now().to_msg()
|
||||
ts.header.frame_id = "world"
|
||||
ts.child_frame_id = part.name
|
||||
ts.transform.translation.x = part.pose.position.x
|
||||
ts.transform.translation.y = part.pose.position.y
|
||||
ts.transform.translation.z = part.pose.position.z
|
||||
ts.transform.rotation.x = part.pose.orientation.x
|
||||
ts.transform.rotation.y = part.pose.orientation.y
|
||||
ts.transform.rotation.z = part.pose.orientation.z
|
||||
ts.transform.rotation.w = part.pose.orientation.w
|
||||
self.tfb_.sendTransform(ts)
|
||||
|
||||
# Populate transforms for relative_part
|
||||
for part in msg.relative_part:
|
||||
ts = TransformStamped()
|
||||
ts.header.stamp = self.get_clock().now().to_msg()
|
||||
ts.header.frame_id = part.relative_at
|
||||
ts.child_frame_id = part.name
|
||||
ts.transform.translation.x = part.pose.position.x
|
||||
ts.transform.translation.y = part.pose.position.y
|
||||
ts.transform.translation.z = part.pose.position.z
|
||||
ts.transform.rotation.x = part.pose.orientation.x
|
||||
ts.transform.rotation.y = part.pose.orientation.y
|
||||
ts.transform.rotation.z = part.pose.orientation.z
|
||||
ts.transform.rotation.w = part.pose.orientation.w
|
||||
self.tfb_.sendTransform(ts)
|
||||
|
||||
# Populate transforms for grasp_pose
|
||||
for part in msg.grasp_pose:
|
||||
ts = TransformStamped()
|
||||
ts.header.stamp = self.get_clock().now().to_msg()
|
||||
ts.header.frame_id = part.relative_at
|
||||
ts.child_frame_id = part.name
|
||||
ts.transform.translation.x = part.pose.position.x
|
||||
ts.transform.translation.y = part.pose.position.y
|
||||
ts.transform.translation.z = part.pose.position.z
|
||||
ts.transform.rotation.x = part.pose.orientation.x
|
||||
ts.transform.rotation.y = part.pose.orientation.y
|
||||
ts.transform.rotation.z = part.pose.orientation.z
|
||||
ts.transform.rotation.w = part.pose.orientation.w
|
||||
self.tfb_.sendTransform(ts)
|
||||
|
||||
|
||||
def main(args=None):
|
||||
rclpy.init(args=args)
|
||||
tf2_publisher_node = TF2PublisherNode()
|
||||
rclpy.spin(tf2_publisher_node)
|
||||
tf2_publisher_node.destroy_node()
|
||||
rclpy.shutdown()
|
||||
|
||||
|
||||
if __name__ == '__main__':
|
||||
main()
|
||||
Loading…
Add table
Add a link
Reference in a new issue