open_pub/Uni-Lab-OS
1
0
Fork 0
mirror of https://github.com/dptech-corp/Uni-Lab-OS.git synced 2025-12-17 04:51:10 +00:00
Code Issues Packages Projects Releases Wiki Activity

添加关节发布节点与物料可视化节点进入unilab

Browse Source
This commit is contained in:
zhangshixiang
2025-04-27 19:07:39 +08:00
parent bed9720de3
commit ff8b75bf1f
6 changed files with 121 additions and 38 deletions
Download Patch File Download Diff File Expand all files Collapse all files

72
unilabos/app/main.py
View File

@@ -59,7 +59,12 @@ def parse_args():
default=None,
help="配置文件路径，支持.py格式的Python配置文件",
)
parser.add_argument(
"--visual",
choices=["rviz", "web","None"],
default="rviz",
help="选择可视化工具: 'rviz' 或 'web' 或 'None'，默认'rviz'",
)
return parser.parse_args()
@@ -152,16 +157,63 @@ def main():
signal.signal(signal.SIGTERM, _exit)
mqtt_client.start()
resource_visualization = ResourceVisualization(args_dict["devices_config"], args_dict["resources_config"],registry_dict)
start_backend(**args_dict)
# print('-'*100)
# print(resource_visualization.resource_model)
# print('-'*100)
server_thread = threading.Thread(target=start_server)
server_thread.start()
resource_visualization.start()
if args_dict["visual"] != "None":
if args_dict["visual"] == "rviz":
resource_visualization = ResourceVisualization(args_dict["devices_config"], args_dict["resources_config"],registry_dict)
elif args_dict["visual"] == "web":
resource_visualization = ResourceVisualization(args_dict["devices_config"], args_dict["resources_config"],registry_dict,enable_rviz=False )
devices_config_add = add_resource_mesh_manager_node(resource_visualization.resource_model, args_dict["resources_config"])
args_dict["devices_config"] = {**args_dict["devices_config"], **devices_config_add}
server_thread = threading.Thread(target=start_server)
server_thread.start()
start_backend(**args_dict)
resource_visualization.start()
else:
start_backend(**args_dict)
start_server()
def add_resource_mesh_manager_node(
resource_model,
resource_config ,
mesh_manager_device_id = "resource_mesh_manager",
joint_publisher_device_id = "joint_republisher"):
mesh_manager_config ={
"id": mesh_manager_device_id,
"name": mesh_manager_device_id,
"children": [],
"parent": None,
"type": "device",
"class": "resource.mesh_manager",
"position": {
"x": 620.6111111111111,
"y": 171,
"z": 0
},
"config": {
"resource_model": resource_model,
"resource_config": resource_config
},
"data": {
}
}
joint_publisher_config = {
"id": joint_publisher_device_id,
"name": joint_publisher_device_id,
"children": [],
"parent": None,
"type": "device",
"class": "joint_republisher",
"position": {
"x": 620.6111111111111,
"y": 171,
"z": 0
},
"config": {},
"data": {}
}
return {joint_publisher_config["id"]: joint_publisher_config,mesh_manager_config["id"]: mesh_manager_config}
# return {joint_publisher_config["id"]: joint_publisher_config}
if __name__ == "__main__":
main()

14
unilabos/device_mesh/resource_visalization.py
View File

@@ -119,12 +119,12 @@ class ResourceVisualization:
}]
)
joint_state_publisher_node = nd(
package='joint_state_publisher_gui', # 或 joint_state_publisher
executable='joint_state_publisher_gui',
name='joint_state_publisher',
output='screen'
)
# joint_state_publisher_node = nd(
# package='joint_state_publisher_gui', # 或 joint_state_publisher
# executable='joint_state_publisher_gui',
# name='joint_state_publisher',
# output='screen'
# )
# 创建move_group节点
move_group = nd(
package='moveit_ros_move_group',
@@ -147,7 +147,7 @@ class ResourceVisualization:
# 将节点添加到launch描述中
self.launch_description.add_action(robot_state_publisher)
self.launch_description.add_action(joint_state_publisher_node)
# self.launch_description.add_action(joint_state_publisher_node)
self.launch_description.add_action(move_group)
# 如果启用RViz,添加RViz节点

17
unilabos/devices/ros_dev/joint_republisher.py
View File

@@ -3,13 +3,22 @@ from rclpy.node import Node
from sensor_msgs.msg import JointState
from std_msgs.msg import String
from rclpy.callback_groups import ReentrantCallbackGroup
from unilabos.ros.nodes.base_device_node import BaseROS2DeviceNode
class JointRepublisher(Node):
def __init__(self,device_id):
super().__init__(device_id)
class JointRepublisher(BaseROS2DeviceNode):
def __init__(self,device_id,resource_tracker):
super().__init__(
driver_instance=self,
device_id=device_id,
status_types={},
action_value_mappings={},
hardware_interface={},
print_publish=False,
resource_tracker=resource_tracker,
)
# print('-'*20,device_id)
self.joint_repub = self.create_publisher(String,f'/devices/{device_id}/joint_state_repub',10)
self.joint_repub = self.create_publisher(String,f'joint_state_repub',10)
# 创建订阅者
self.create_subscription(
JointState,

41
unilabos/devices/ros_dev/resource_mesh_manager.py
View File

@@ -19,18 +19,27 @@ from tf2_ros import TransformBroadcaster, Buffer, TransformListener
from rclpy.action import ActionServer
from unilabos_msgs.action import SendCmd
from rclpy.action.server import ServerGoalHandle
from unilabos.ros.nodes.base_device_node import BaseROS2DeviceNode,DeviceNodeResourceTracker
class ResourceMeshManager(Node):
def __init__(self, resource_model: dict, resource_config: list, node_name: str):
class ResourceMeshManager(BaseROS2DeviceNode):
def __init__(self, resource_model: dict, resource_config: list,resource_tracker, device_id: str = "resource_mesh_manager"):
"""初始化资源网格管理器节点
Args:
resource_model (dict): 资源模型字典,包含资源的3D模型信息
resource_config (dict): 资源配置字典,包含资源的配置信息
node_name (str): 节点名称
device_id (str): 节点名称
"""
super().__init__(node_name)
super().__init__(
driver_instance=self,
device_id=device_id,
status_types={},
action_value_mappings={},
hardware_interface={},
print_publish=False,
resource_tracker=resource_tracker,
)
self.resource_model = resource_model
self.resource_config_dict = {item['id']: item for item in resource_config}
self.move_group_ready = False
@@ -38,7 +47,7 @@ class ResourceMeshManager(Node):
self.tf_broadcaster = TransformBroadcaster(self)
self.tf_buffer = Buffer()
self.tf_listener = TransformListener(self.tf_buffer, self)
self.old_resource_pose = {}
self.__planning_scene = None
@@ -49,7 +58,7 @@ class ResourceMeshManager(Node):
callback_group = ReentrantCallbackGroup()
self._get_planning_scene_service = self.create_client(
srv_type=GetPlanningScene,
srv_name="get_planning_scene",
srv_name="/get_planning_scene",
qos_profile=QoSProfile(
durability=QoSDurabilityPolicy.VOLATILE,
reliability=QoSReliabilityPolicy.RELIABLE,
@@ -62,7 +71,7 @@ class ResourceMeshManager(Node):
# Create a service for applying the planning scene
self._apply_planning_scene_service = self.create_client(
srv_type=ApplyPlanningScene,
srv_name="apply_planning_scene",
srv_name="/apply_planning_scene",
qos_profile=QoSProfile(
durability=QoSDurabilityPolicy.VOLATILE,
reliability=QoSReliabilityPolicy.RELIABLE,
@@ -73,7 +82,7 @@ class ResourceMeshManager(Node):
)
self.resource_pose_publisher = self.create_publisher(
String, f"devices/{node_name}/resource_pose", 10
String, f"resource_pose", 10
)
self.__collision_object_publisher = self.create_publisher(
CollisionObject, "/collision_object", 10
@@ -86,7 +95,7 @@ class ResourceMeshManager(Node):
self._action_server = ActionServer(
self,
SendCmd,
f'devices/{node_name}/tf_update',
f"tf_update",
self.tf_update,
callback_group=callback_group
)
@@ -94,6 +103,7 @@ class ResourceMeshManager(Node):
self.resource_mesh_setup()
self.create_timer(0.02, self.publish_resource_tf)
self.create_timer(1, self.check_resource_pose_changes)
def check_move_group_ready(self):
@@ -101,8 +111,9 @@ class ResourceMeshManager(Node):
# 获取当前可用的节点列表
tf_ready = self.tf_buffer.can_transform("world", next(iter(self.resource_tf_dict.keys())), rclpy.time.Time())
# print(tf_ready)
tf_ready = self.tf_buffer.can_transform("world", next(iter(self.resource_tf_dict.keys())), rclpy.time.Time(),rclpy.duration.Duration(seconds=2))
# if tf_ready:
if self._get_planning_scene_service.service_is_ready() and self._apply_planning_scene_service.service_is_ready() and tf_ready:
self.move_group_ready = True
self.add_resource_collision_meshes()
@@ -5022,10 +5033,10 @@ if __name__ == '__main__':
'''
resource_config = json.loads(config_s.replace("'",'"'))
rclpy.init()
resource_mesh_manager = ResourceMeshManager(resource_model, resource_config, 'resource_mesh_manager')
resource_mesh_manager = ResourceMeshManager(resource_model, resource_config, DeviceNodeResourceTracker())
# resource_mesh_manager.resource_mesh_setup()
resource_mesh_manager.check_move_group_ready()
resource_mesh_manager.publish_resource_tf()
# resource_mesh_manager.check_move_group_ready()
# resource_mesh_manager.publish_resource_tf()
# resource_mesh_manager.clear_all_collision_objects()
# print(json.dumps(resource_mesh_manager.resource_tf_dict, indent=4, ensure_ascii=False))

11
unilabos/registry/devices/sim_nodes.yaml Normal file
View File

@@ -0,0 +1,11 @@
joint_republisher:
class:
module: unilabos.devices.ros_dev.joint_republisher:JointRepublisher
type: ros2
resource.mesh_manager:
class:
module: unilabos.devices.ros_dev.resource_mesh_manager:ResourceMeshManager
type: ros2