ROS2 轨迹规划核心点
核心：
0、 创建节点
rosNode=std::make_shared<rclcpp::Node>("robot_server_node");
1、持续发布机械臂当前状态节点 （基础节点，待补充）
// 创建发布器，发布到 /joint_states 话题
joint_state_publisher_ = rosNode->create_publisher<sensor_msgs::msg::JointState>("joint_states", 10);
joint_state_publisher_->publish(joint_state_msg);
2、实现发布点云功能
-------- 初始化 （对这个节点 初始化发布点云功能） --------
pub_ = rosNode->create_publisher<sensor_msgs::msg::PointCloud2>("/sensor/point_cloud", 1);
------- 发布点云功能 -------
sensor_msgs::msg::PointCloud2 output;
pcl::toROSMsg(*cloud, output);
output.header.stamp = rosNode->now();
output.header.frame_id = frame_id_;
pub_->publish(output);
-------- 在轨迹规划中，应用点云 ----------
// 自动添加
在 moveit 助手生成的配置里新增 sensors_3d.yaml ，这样就会自动应用到 轨迹规划里了。
sensors:
- default_sensor
default_sensor:
filtered_cloud_topic: /sensor/filtered_points
max_range: 5.0
max_update_rate: 1.0
padding_offset: 0.1
padding_scale: 1.0
point_cloud_topic: /sensor/point_cloud
point_subsample: 1
sensor_plugin: occupancy_map_monitor/PointCloudOctomapUpdater
3、实现 当前关节状态 到 目标位姿 的 轨迹规划
-------- 初始化 运动规划 实例对象 ------------
move_group_ = std::make_shared<moveit::planning_interface::MoveGroupInterface>(rosNode, planning_group_name_);
// set some defaults similar to your python script
move_group_->setGoalPositionTolerance(0.02);
move_group_->setGoalOrientationTolerance(0.03);
move_group_->setMaxVelocityScalingFactor(0.8);
move_group_->setPlanningTime(10.0);
move_group_->setNumPlanningAttempts(3);
-------- 实现轨迹规划 -------
//设置规划的起点
move_group_->setStartState(start_state); （moveit_msgs::msg::RobotState start_state）
// 规划已知关节 到 已知目标位姿 的路径
// 设置目标终点的位姿 xyz+四元数
move_group_->setPoseTarget(target_ps, eef_link_); （geometry_msgs::msg::PoseStamped target_ps）
// 执行规划
moveit::planning_interface::MoveGroupInterface::Plan out_plan; （声明一个变量，存放输出结果）
move_group_->plan(out_plan)