add workspace publisher

rbs_mill_assist/package.xml

@@ -26,6 +26,7 @@
   <test_depend>ament_lint_common</test_depend>
 
   <export>
+    <rviz plugin="${prefix}/materials/plugins.xml"/>
     <build_type>ament_cmake</build_type>
   </export>
 </package>

rbs_mill_assist/src/get_workspace.cpp

@@ -0,0 +1,202 @@
+#include <Eigen/Dense>
+#include <Eigen/src/Core/Matrix.h>
+#include <fstream>
+#include <memory>
+#include <pinocchio/algorithm/joint-configuration.hpp>
+#include <pinocchio/algorithm/frames.hpp>
+#include <pinocchio/algorithm/kinematics.hpp>
+#include <pinocchio/parsers/urdf.hpp>
+#include <rclcpp/logging.hpp>
+#include <rclcpp/rclcpp.hpp>
+#include <sensor_msgs/msg/point_cloud2.hpp>
+#include <sensor_msgs/msg/point_field.hpp>
+#include <string>
+#include <urdf_model/model.h>
+#include <urdf_parser/urdf_parser.h>
+#include <vector>
+
+using namespace std;
+using namespace pinocchio;
+using sensor_msgs::msg::PointCloud2;
+using sensor_msgs::msg::PointField;
+
+class WorkspaceCalculator : public rclcpp::Node {
+public:
+  WorkspaceCalculator() : Node("workspace_calculator") {
+    this->declare_parameter("urdf_path", "");
+    this->declare_parameter("ee_link", "");
+    this->declare_parameter("joint_resolution", 7);
+    this->declare_parameter("robot_position", std::vector<double>({0.0, 0.0, 0.0}));
+
+
+    robot_position = this->get_parameter("robot_position").as_double_array();
+    joint_resolution_ = this->get_parameter("joint_resolution").as_int();
+    ee_link_ = this->get_parameter("ee_link").as_string();
+    urdf_path_ = this->get_parameter("urdf_path").as_string();
+
+    publisher_ = this->create_publisher<PointCloud2>("workspace", 10);
+
+    compute_workspace();
+    timer_ = this->create_wall_timer(
+        std::chrono::seconds(1),
+        std::bind(&WorkspaceCalculator::publish_pointcloud, this));
+  }
+
+private:
+  rclcpp::Publisher<PointCloud2>::SharedPtr publisher_;
+  rclcpp::TimerBase::SharedPtr timer_;
+  std::string urdf_path_;
+  std::string ee_link_;
+  std::vector<double> robot_position{};
+  int joint_resolution_;
+  std::vector<Eigen::Vector3d> workspace_points_;
+
+  void compute_workspace() {
+    if (urdf_path_.empty() || ee_link_.empty()) {
+      RCLCPP_ERROR(this->get_logger(),
+                   "URDF path and end-effector link must be provided.");
+      return;
+    }
+
+    Model model;
+    pinocchio::urdf::buildModel(urdf_path_, model);
+    Data data(model);
+    auto urdf_robot = ::urdf::parseURDFFile(urdf_path_);
+
+    auto joint_limits = get_joint_limits(urdf_robot, model);
+    auto joint_grid = create_joint_grid(joint_limits, joint_resolution_);
+
+
+
+    for (const Eigen::VectorXd &q : joint_grid) {
+      forwardKinematics(model, data, q);
+      updateFramePlacements(model, data);
+ 
+      auto ee_index = model.getFrameId(ee_link_);
+      if (ee_index < 0) {
+        RCLCPP_ERROR(this->get_logger(),
+                     "End-effector link not found in model.");
+      }
+
+
+      auto ee_pos = data.oMf[ee_index].translation();
+      workspace_points_.emplace_back(ee_pos);
+
+      if (ee_pos.isApprox(Eigen::Vector3d::Zero(), 1e-6)) {
+          continue; // Пропускаем нулевую позицию
+      }
+
+    }
+    RCLCPP_INFO(this->get_logger(), "Workspace calculation has been ended");
+  }
+
+  std::vector<std::pair<double, double>>
+  get_joint_limits(const std::shared_ptr<::urdf::ModelInterface> &urdf_robot,
+                   const Model &model) {
+    std::vector<std::pair<double, double>> joint_limits;
+
+    for (const auto &joint : urdf_robot->joints_) {
+      if (joint.second->limits) {
+        joint_limits.emplace_back(joint.second->limits->lower,
+                                  joint.second->limits->upper);
+      }
+    }
+
+    return joint_limits;
+  }
+
+  std::vector<Eigen::VectorXd>
+  create_joint_grid(const std::vector<std::pair<double, double>> &joint_limits,
+                    int resolution) {
+    std::vector<Eigen::VectorXd> grid;
+    std::vector<std::vector<double>> joint_values;
+
+    for (const auto &lim : joint_limits) {
+      std::vector<double> range;
+      double step = (lim.second - lim.first) / (resolution - 1);
+      for (int i = 0; i < resolution; ++i) {
+        range.push_back(lim.first + i * step);
+      }
+      joint_values.push_back(range);
+    }
+
+    size_t num_joints = joint_limits.size();
+    std::vector<size_t> indices(num_joints, 0);
+    while (true) {
+      Eigen::VectorXd q(num_joints);
+      for (size_t i = 0; i < num_joints; ++i) {
+        q[i] = joint_values[i][indices[i]];
+      }
+      grid.push_back(q);
+
+      for (size_t i = 0; i < num_joints; ++i) {
+        if (++indices[i] < resolution)
+          break;
+        if (i == num_joints - 1)
+          return grid;
+        indices[i] = 0;
+      }
+    }
+  }
+
+  void publish_pointcloud() {
+    if (workspace_points_.empty()) {
+      RCLCPP_ERROR(this->get_logger(), "No workspace points computed.");
+      return;
+    }
+
+    PointCloud2 msg;
+    msg.header.stamp = this->now();
+    msg.header.frame_id = "base_link";
+    msg.height = 1;
+    msg.width = workspace_points_.size();
+    msg.is_dense = false;
+    msg.is_bigendian = false;
+
+    sensor_msgs::msg::PointField field_x;
+    field_x.name = "x";
+    field_x.offset = 0;
+    field_x.datatype = sensor_msgs::msg::PointField::FLOAT32;
+    field_x.count = 1;
+    msg.fields.push_back(field_x);
+
+    sensor_msgs::msg::PointField field_y;
+    field_y.name = "y";
+    field_y.offset = 4;
+    field_y.datatype = sensor_msgs::msg::PointField::FLOAT32;
+    field_y.count = 1;
+    msg.fields.push_back(field_y);
+
+    sensor_msgs::msg::PointField field_z;
+    field_z.name = "z";
+    field_z.offset = 8;
+    field_z.datatype = sensor_msgs::msg::PointField::FLOAT32;
+    field_z.count = 1;
+    msg.fields.push_back(field_z);
+
+    msg.point_step = 12;
+    msg.row_step = msg.point_step * msg.width;
+    msg.data.resize(msg.row_step);
+
+    uint8_t *data_ptr = msg.data.data();
+    for (const auto &p : workspace_points_) {
+      // RCLCPP_INFO(this->get_logger(), "Point: (%f, %f, %f)", p.x(), p.y(),
+      //             p.z());
+      float x = p.x() - robot_position[0], y = p.y() - robot_position[1], z = p.z() - robot_position[2];
+      memcpy(data_ptr, &x, 4);
+      memcpy(data_ptr + 4, &y, 4);
+      memcpy(data_ptr + 8, &z, 4);
+      data_ptr += 12;
+    }
+
+    publisher_->publish(msg);
+  }
+};
+
+int main(int argc, char **argv) {
+  rclcpp::init(argc, argv);
+  auto node = std::make_shared<WorkspaceCalculator>();
+  rclcpp::spin(node);
+  rclcpp::shutdown();
+  return 0;
+}