Обновлены параметры планирования и добавлены новые файлы для модуля планирования движения, включая C++ и Python реализации.

src/iiwa_planning/CMakeLists.txt

@@ -10,11 +10,32 @@ find_package(ament_cmake REQUIRED)
 find_package(ament_cmake_python REQUIRED)
 find_package(rclcpp REQUIRED)
 find_package(rclpy REQUIRED)
+find_package(moveit_ros_planning_interface REQUIRED)
+find_package(moveit_core REQUIRED)
+find_package(moveit_ros_planning REQUIRED)
+find_package(moveit_msgs REQUIRED)
+find_package(geometry_msgs REQUIRED)
+
+add_executable(motion_planning_cpp src/motion_planning_cpp.cpp)
+target_include_directories(motion_planning_cpp PUBLIC include)
+
+ament_target_dependencies(motion_planning_cpp
+  rclcpp
+  moveit_ros_planning_interface
+  moveit_core
+  moveit_ros_planning
+  moveit_msgs
+  geometry_msgs
+)
 
-include_directories(include)
 
 ament_python_install_package(${PROJECT_NAME})
 
+install(TARGETS
+  motion_planning_cpp
+  DESTINATION lib/${PROJECT_NAME}
+)
+
 install(PROGRAMS
   # scripts/motion_planning_test.py
   scripts/motion_planning.py

src/iiwa_planning/scripts/motion_planning.py

@@ -14,9 +14,10 @@ class MotionPlaning(Node):
         super().__init__("motion_planning_node")
 
         self.moveit = MoveItPy(node_name="motion_planning_node")
-        self.robot_arm = self.moveit.get_planning_component("iiwa_arm")
+        self.robot_arm: PlanningComponent = self.moveit.get_planning_component("iiwa_arm")
         self.planning_scene_monitor = self.moveit.get_planning_scene_monitor()
 
+        
         self.get_logger().warn("***********************************************************")
         self.get_logger().info("MotionPlanning node is ready...")
         self.get_logger().warn("***********************************************************")

src/iiwa_planning/src/motion_planning_cpp.cpp

@@ -0,0 +1,58 @@
+#include <chrono>
+#include <memory>
+#include <string>
+
+#include <pluginlib/class_loader.hpp>
+
+#include <rclcpp/rclcpp.hpp>
+#include <moveit/robot_model_loader/robot_model_loader.hpp>
+#include <moveit/planning_pipeline/planning_pipeline.hpp>
+#include <moveit/planning_scene/planning_scene.hpp>
+#include <moveit/kinematic_constraints/utils.hpp>
+#include <moveit_msgs/msg/display_trajectory.hpp>
+#include <moveit_msgs/msg/planning_scene.hpp>
+#include <moveit/move_group_interface/move_group_interface.hpp>
+
+
+class MotionPlanning: public rclcpp::Node 
+{
+    public:
+        MotionPlanning() : Node("motion_planning_node",
+                                rclcpp::NodeOptions().automatically_declare_parameters_from_overrides(true))
+        {
+            RCLCPP_INFO(get_logger(), "***************");
+            RCLCPP_INFO(get_logger(), "MotionPlaning node created");
+            RCLCPP_INFO(get_logger(), "***************");
+
+            timer_ = create_wall_timer(
+                std::chrono::milliseconds(200),
+                std::bind(&MotionPlanning::runOnce, this)
+            );            
+
+        }
+
+    private:
+        void runOnce() 
+        {
+            timer_->cancel();
+            
+        }
+
+    private:
+        rclcpp::TimerBase::SharedPtr timer_;
+
+        const std::string planning_group_ = "iiwa_arm";
+        const std::string base_frame_ = "base_link";
+        const std::string ee_link = "link_ee";
+
+
+};
+
+
+int main(int argc, char * argv[])
+{
+    rclcpp::init(argc, argv);
+    rclcpp::spin(std::make_shared<MotionPlanning>());
+    rclcpp::shutdown();
+    return 0;
+}