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()
|
||||
36
rbs_bt_executor/bt_trees/test_roboclone.xml
Normal file
36
rbs_bt_executor/bt_trees/test_roboclone.xml
Normal file
|
|
@ -0,0 +1,36 @@
|
|||
<?xml version="1.0"?>
|
||||
<root main_tree_to_execute="MainTree">
|
||||
<!-- ////////// -->
|
||||
<BehaviorTree ID="MainTree">
|
||||
<Sequence>
|
||||
<Action ID="EnvStarter" env_class="gz_enviroment::GzEnviroment" env_name="gz_enviroment"
|
||||
server_name="/env_manager/start_env" server_timeout="1000" workspace="{workspace}" />
|
||||
<SubTreePlus ID="WorkspaceInspection" __autoremap="1" goal_pose="{workspace}"
|
||||
robot_name="rbs_arm" />
|
||||
</Sequence>
|
||||
</BehaviorTree>
|
||||
<!-- ////////// -->
|
||||
<BehaviorTree ID="WorkspaceInspection">
|
||||
<Sequence>
|
||||
<Action ID="MoveToPoseArray" cancel_timeout="5000" pose_vec_in="{goal_pose}"
|
||||
pose_vec_out="{goal_pose}"
|
||||
robot_name="{robot_name}" server_name="/arm1/move_topose" server_timeout="5000" />
|
||||
<Action ID="MoveToPoseArray" cancel_timeout="5000" pose_vec_in="{goal_pose}"
|
||||
pose_vec_out="{goal_pose}"
|
||||
robot_name="{robot_name}" server_name="/arm1/move_cartesian" server_timeout="5000" />
|
||||
<Action ID="MoveToPoseArray" cancel_timeout="5000" pose_vec_in="{goal_pose}"
|
||||
pose_vec_out="{goal_pose}"
|
||||
robot_name="{robot_name}" server_name="/arm1/move_cartesian" server_timeout="5000" />
|
||||
<Action ID="MoveToPoseArray" cancel_timeout="5000" pose_vec_in="{goal_pose}"
|
||||
pose_vec_out="{goal_pose}"
|
||||
robot_name="{robot_name}" server_name="/arm1/move_cartesian" server_timeout="5000" />
|
||||
<Action ID="MoveToPoseArray" cancel_timeout="5000" pose_vec_in="{goal_pose}"
|
||||
pose_vec_out="{goal_pose}"
|
||||
robot_name="{robot_name}" server_name="/arm/move_cartesian" server_timeout="5000" />
|
||||
|
||||
</Sequence>
|
||||
</BehaviorTree>
|
||||
<!-- ////////// -->
|
||||
<include path="./nodes_interfaces/general.xml"/>
|
||||
<!-- ////////// -->
|
||||
</root>
|
||||
108
rbs_skill_servers/scripts/move_to_pose.py
Executable file
108
rbs_skill_servers/scripts/move_to_pose.py
Executable file
|
|
@ -0,0 +1,108 @@
|
|||
#!/usr/bin/python
|
||||
import rclpy
|
||||
from rclpy.action import ActionServer
|
||||
from rclpy.node import Node
|
||||
import numpy as np
|
||||
from rclpy.callback_groups import ReentrantCallbackGroup
|
||||
from rclpy.executors import MultiThreadedExecutor
|
||||
|
||||
from geometry_msgs.msg import Pose, PoseStamped
|
||||
from rbs_skill_interfaces.action import MoveitSendPose
|
||||
|
||||
class PoseSubscriber(Node):
|
||||
def __init__(self, parent=None):
|
||||
super().__init__('pose_subscriber')
|
||||
self.parent = parent
|
||||
self._sub = self.create_subscription(PoseStamped,
|
||||
"/cartesian_motion_controller/current_pose",
|
||||
self.parent.on_pose_callback, 1,
|
||||
callback_group=self.parent._callback_group)
|
||||
self.get_logger().info('PoseSubscriber node initialized')
|
||||
|
||||
class CartesianMoveToPose(Node):
|
||||
|
||||
def __init__(self):
|
||||
super().__init__('cartesian_move_to_pose')
|
||||
self._callback_group = ReentrantCallbackGroup()
|
||||
self._action_server = ActionServer(
|
||||
self,
|
||||
MoveitSendPose,
|
||||
'cartesian_move_to_pose',
|
||||
self.execute_callback, callback_group=self._callback_group)
|
||||
self._pub = self.create_publisher(PoseStamped,
|
||||
"/cartesian_motion_controller/target_frame", 1,
|
||||
callback_group=self._callback_group)
|
||||
self.current_pose = None
|
||||
self.goal_tolerance = 0.05
|
||||
|
||||
def on_pose_callback(self, msg: PoseStamped):
|
||||
if isinstance(msg, PoseStamped):
|
||||
self.current_pose = msg
|
||||
|
||||
def execute_callback(self, goal_handle):
|
||||
self.get_logger().debug(f"Executing goal {goal_handle.request.target_pose}")
|
||||
tp = PoseStamped()
|
||||
tp.pose = goal_handle.request.target_pose
|
||||
tp.header.stamp = self.get_clock().now().to_msg()
|
||||
tp.header.frame_id = "base_link"
|
||||
|
||||
while self.get_distance_to_target(tp.pose) >= self.goal_tolerance:
|
||||
self._pub.publish(tp)
|
||||
|
||||
goal_handle.succeed()
|
||||
|
||||
result = MoveitSendPose.Result()
|
||||
result.success = True
|
||||
return result
|
||||
|
||||
def get_distance_to_target(self, target_pose: Pose):
|
||||
if self.current_pose is None or self.current_pose.pose is None:
|
||||
self.get_logger().warn("Current pose is not available")
|
||||
return None
|
||||
|
||||
current_pose = self.current_pose.pose
|
||||
|
||||
current_position = np.array([
|
||||
current_pose.position.x,
|
||||
current_pose.position.y,
|
||||
current_pose.position.z,
|
||||
current_pose.orientation.x,
|
||||
current_pose.orientation.y,
|
||||
current_pose.orientation.z
|
||||
])
|
||||
|
||||
target_position = np.array([
|
||||
target_pose.position.x,
|
||||
target_pose.position.y,
|
||||
target_pose.position.z,
|
||||
target_pose.orientation.x,
|
||||
target_pose.orientation.y,
|
||||
target_pose.orientation.z
|
||||
])
|
||||
|
||||
# Проверка на наличие значений в массивах координат
|
||||
if np.any(np.isnan(current_position)) or np.any(np.isnan(target_position)):
|
||||
self.get_logger().error("Invalid coordinates")
|
||||
return None
|
||||
|
||||
# Вычисляем расстояние между текущей и целевой позициями
|
||||
distance = np.linalg.norm(current_position - target_position)
|
||||
|
||||
return distance
|
||||
|
||||
def main(args=None):
|
||||
rclpy.init(args=args)
|
||||
|
||||
cartesian_move_to_pose = CartesianMoveToPose()
|
||||
pose_subscriber = PoseSubscriber(parent=cartesian_move_to_pose)
|
||||
|
||||
executor = MultiThreadedExecutor()
|
||||
executor.add_node(cartesian_move_to_pose)
|
||||
executor.add_node(pose_subscriber)
|
||||
|
||||
executor.spin()
|
||||
|
||||
rclpy.shutdown()
|
||||
|
||||
if __name__ == '__main__':
|
||||
main()
|
||||
Loading…
Add table
Add a link
Reference in a new issue