from rclpy.node import Node
from rclpy.callback_groups import ReentrantCallbackGroup
from rclpy.executors import MultiThreadedExecutor
from moveit_msgs.srv import GetMotionPlan
from moveit_msgs.msg import (
    Constraints, PositionConstraint, OrientationConstraint,
    BoundingVolume, RobotState,
)
from sensor_msgs.msg import JointState
from shape_msgs.msg import SolidPrimitive

class TrajectoryServer(Node):
    def __init__(self):
        super().__init__("trajectory_server")
        self.callback_group = ReentrantCallbackGroup()

        # 订阅 joint_states 获取当前状态
        self.current_joint_state = None
        self.joint_sub = self.create_subscription(
            JointState, "/joint_states", self.joint_state_callback, 10
        )

        self.client = self.create_client(
            GetMotionPlan, "/plan_kinematic_path",
            callback_group=self.callback_group
        )

    def plan(self, target_pose, group_name="arm", ee_link="link6", base_frame="base_link"):
        req = GetMotionPlan.Request()
        req.motion_plan_request.group_name = group_name
        req.motion_plan_request.num_planning_attempts = 10
        req.motion_plan_request.allowed_planning_time = 5.0

        # 设置起始状态（重要！）
        if self.current_joint_state:
            req.motion_plan_request.start_state = RobotState()
            req.motion_plan_request.start_state.joint_state = self.current_joint_state

        # 位置约束
        pos = PositionConstraint()
        pos.header.frame_id = base_frame
        pos.link_name = ee_link
        pos.constraint_region = BoundingVolume()
        sphere = SolidPrimitive(type=SolidPrimitive.SPHERE, dimensions=[0.01])
        pos.constraint_region.primitives.append(sphere)
        pos.constraint_region.primitive_poses.append(target_pose)
        pos.weight = 1.0

        # 姿态约束
        ori = OrientationConstraint()
        ori.header.frame_id = base_frame
        ori.link_name = ee_link
        ori.orientation = target_pose.orientation
        ori.absolute_x_axis_tolerance = 0.1
        ori.absolute_y_axis_tolerance = 0.1
        ori.absolute_z_axis_tolerance = 0.1
        ori.weight = 1.0

        goal = Constraints()
        goal.position_constraints.append(pos)
        goal.orientation_constraints.append(ori)
        req.motion_plan_request.goal_constraints.append(goal)

        future = self.client.call_async(req)
        rclpy.spin_until_future_complete(self, future)
        result = future.result()

        if result.motion_plan_response.error_code.val == 1:  # SUCCESS
            return result.motion_plan_response.trajectory.joint_trajectory
        return None

# 必须使用 MultiThreadedExecutor
executor = MultiThreadedExecutor()
executor.add_node(node)
executor.spin()

moveit_config = (
    MoveItConfigsBuilder("your_robot")
    .sensors_3d(file_path="config/sensors_3d.yaml")
    .to_moveit_configs()
)

from moveit_msgs.msg import PlanningScene, CollisionObject
from shape_msgs.msg import SolidPrimitive

collision_object = CollisionObject()
collision_object.header.frame_id = 'base_link'
collision_object.id = 'obstacle'

box = SolidPrimitive()
box.type = SolidPrimitive.BOX
box.dimensions = [0.1, 0.1, 0.5]

collision_object.primitives.append(box)
collision_object.primitive_poses.append(pose)
collision_object.operation = CollisionObject.ADD

scene = PlanningScene()
scene.is_diff = True
scene.world.collision_objects.append(collision_object)

planning_scene_publisher.publish(scene)

from rclpy.callback_groups import ReentrantCallbackGroup
from rclpy.executors import MultiThreadedExecutor

self.callback_group = ReentrantCallbackGroup()
self.client = self.create_client(srv_type, srv_name, callback_group=self.callback_group)
self.srv = self.create_service(srv_type, srv_name, callback, callback_group=self.callback_group)

executor = MultiThreadedExecutor()
executor.add_node(node)
executor.spin()

data_files=[
    ('share/ament_index/resource_index/packages', ['resource/my_pkg']),
    ('share/' + package_name, ['package.xml']),
    ('share/' + package_name + '/launch', glob('launch/*.py')),
]

from launch import LaunchDescription
from launch_ros.actions import Node

def generate_launch_description():
    return LaunchDescription([
        Node(
            package='package_one',
            executable='node_one',
            name='custom_name',           # 可选，覆盖默认节点名
            parameters=[{'param1': 'value1'}],
            remappings=[('/old_topic', '/new_topic')],
            output='screen',              # 输出到终端
        ),
        Node(
            package='package_two',
            executable='node_two',
        ),
    ])

from launch.actions import IncludeLaunchDescription
from launch.launch_description_sources import PythonLaunchDescriptionSource
from ament_index_python.packages import get_package_share_directory
import os

def generate_launch_description():
    other_launch = IncludeLaunchDescription(
        PythonLaunchDescriptionSource(
            os.path.join(get_package_share_directory('other_pkg'),
                        'launch', 'other.launch.py')
        )
    )
    return LaunchDescription([other_launch])

# Python
from my_interfaces.msg import MyMessage
from my_interfaces.srv import MyService
from my_interfaces.action import MyAction