packages for engineer2026

auto_exchange_planner/CMakeLists.txt

@@ -0,0 +1,79 @@
+cmake_minimum_required(VERSION 3.10)
+project(auto_exchange_planner)
+
+set(CMAKE_CXX_STANDARD 14)
+set(CMAKE_CXX_STANDARD_REQUIRED ON)
+
+add_definitions(-Wall -Werror)
+
+find_package(catkin REQUIRED COMPONENTS
+	moveit_core
+	pluginlib
+	roscpp
+	std_msgs
+)
+
+find_package(Eigen3 REQUIRED)
+
+catkin_package(
+	INCLUDE_DIRS
+	include
+	${EIGEN3_INCLUDE_DIR}
+  	LIBRARIES
+  	CATKIN_DEPENDS
+	roscpp
+	std_msgs
+  	DEPENDS
+)
+
+include_directories(include)
+include_directories(
+	SYSTEM
+	${catkin_INCLUDE_DIRS}
+	${Boost_INCLUDE_DIRS}
+)
+
+#add_executable(test_auto_exchange  src/test_auto_exchange.cpp)
+#target_link_libraries(test_auto_exchange
+#	${catkin_LIBRARIES}
+#	${Boost_LIBRARIES}
+#)
+
+add_library(moveit_auto_exchange_planner_plugin
+	src/auto_exchange_manager.cpp
+	src/auto_exchange_planner.cpp
+	src/auto_exchange_context.cpp)
+set_target_properties(moveit_auto_exchange_planner_plugin PROPERTIES VERSION "${${PROJECT_NAME}_VERSION}")
+target_link_libraries(moveit_auto_exchange_planner_plugin ${catkin_LIBRARIES} ${Boost_LIBRARIES})
+
+# Mark executables and/or libraries for installation
+install(
+	TARGETS
+	moveit_auto_exchange_planner_plugin
+	ARCHIVE DESTINATION
+	${CATKIN_PACKAGE_LIB_DESTINATION}
+	LIBRARY DESTINATION
+	${CATKIN_PACKAGE_LIB_DESTINATION}
+	RUNTIME DESTINATION
+	${CATKIN_PACKAGE_BIN_DESTINATION}
+)
+
+# Mark cpp header files for installation
+install(
+	DIRECTORY
+	include
+	DESTINATION
+	${CATKIN_PACKAGE_INCLUDE_DESTINATION}
+)
+
+# Mark roslaunch files for installation
+install(
+	DIRECTORY
+	launch
+	DESTINATION
+	${CATKIN_PACKAGE_SHARE_DESTINATION}
+)
+#catkin_lint ignore uninstalled_plugin
+install(FILES auto_exchange_planner_plugin_description.xml
+	DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION})
+

auto_exchange_planner/auto_exchange_planner_plugin_description.xml

@@ -0,0 +1,7 @@
+<library path="libmoveit_auto_exchange_planner_plugin">
+  <class name="auto_exchange_planner/AutoExchangePlanner" type="auto_exchange_planner::AutoExchangeManager" base_class_type="planning_interface::PlannerManager">
+    <description>
+      The motion planner plugin which plans a particular path for RM exchanger.
+    </description>
+  </class>
+</library>

auto_exchange_planner/include/auto_exchange_planner/auto_exchange_context.h

@@ -0,0 +1,35 @@
+//
+// Created by ch on 24-12-6.
+//
+
+#pragma once
+
+#include <moveit/planning_interface/planning_interface.h>
+#include "auto_exchange_planner/auto_exchange_planner.h"
+
+namespace auto_exchange_planner
+{
+  MOVEIT_CLASS_FORWARD(AutoExchangeContext);
+
+  class AutoExchangeContext : public planning_interface::PlanningContext
+  {
+  public:
+    AutoExchangeContext(const std::string& name, const std::string& ns, const std::string& group,
+                        const moveit::core::RobotModelConstPtr& model);
+    ~AutoExchangeContext() override
+    {
+    }
+
+    bool solve(planning_interface::MotionPlanResponse& res) override;
+    bool solve(planning_interface::MotionPlanDetailedResponse& res) override;
+
+    bool terminate() override;
+    void clear() override;
+
+  private:
+    moveit::core::RobotModelConstPtr robot_model_;
+    moveit::core::RobotStatePtr robot_state_;
+    AutoExchangePlannerPtr auto_exchange_planner_;
+  };
+
+}  // namespace auto_exchange_planner

auto_exchange_planner/include/auto_exchange_planner/auto_exchange_planner.h

@@ -0,0 +1,37 @@
+//
+// Created by ch on 24-12-1.
+//
+
+#pragma once
+
+#include <ros/ros.h>
+#include <moveit/planning_interface/planning_interface.h>
+
+namespace auto_exchange_planner
+{
+  MOVEIT_CLASS_FORWARD(AutoExchangePlanner);
+
+  class AutoExchangePlanner
+  {
+  public:
+    AutoExchangePlanner(const ros::NodeHandle& nh = ros::NodeHandle("~"));
+
+    bool solve(const planning_scene::PlanningSceneConstPtr& planning_scene,
+               const planning_interface::MotionPlanRequest& req, moveit_msgs::MotionPlanDetailedResponse& res);
+
+  protected:
+    ros::NodeHandle nh_;
+    std::string name_;
+    int num_steps_;
+    int dof_;
+    double trajectory2_length_;
+
+  private:
+    void interpolate(const std::vector<std::string>& joint_names, moveit::core::RobotStatePtr& robot_state,
+                     const moveit::core::JointModelGroup* joint_model_group, const std::vector<double>& start_joint_vals,
+                     const std::vector<double>& goal_joint_vals, trajectory_msgs::JointTrajectory& joint_trajectory);
+    void auto_exchange_interpolate(const std::vector<std::string>& joint_names, moveit::core::RobotStatePtr& robot_state,
+                 const moveit::core::JointModelGroup* joint_model_group, const geometry_msgs::Pose& start_pose,
+                 const geometry_msgs::Pose& goal_pose, trajectory_msgs::JointTrajectory& joint_trajectory);
+  };
+}  // namespace auto_exchange_planner

auto_exchange_planner/package.xml

@@ -0,0 +1,22 @@
+<?xml version='1.0' encoding='UTF-8'?>
+<package format="2">
+  <name>auto_exchange_planner</name>
+  <version>0.0.0</version>
+  <description>The auto_exchange_planner package</description>
+  <maintainer email="3631676002@qq.com">ch</maintainer>
+  <license>BSD</license>
+  <buildtool_depend>catkin</buildtool_depend>
+  <build_depend>pluginlib</build_depend>
+  <build_depend>moveit_core</build_depend>
+  <build_depend>roscpp</build_depend>
+  <build_depend>std_msgs</build_depend>
+  <exec_depend>pluginlib</exec_depend>
+  <exec_depend>moveit_core</exec_depend>
+  <exec_depend>roscpp</exec_depend>
+  <exec_depend>std_msgs</exec_depend>
+
+
+  <export>
+    <moveit_core plugin="${prefix}/auto_exchange_planner_plugin_description.xml"/>
+  </export>
+</package>

auto_exchange_planner/src/auto_exchange_context.cpp

@@ -0,0 +1,72 @@
+//
+// Created by ch on 24-12-6.
+//
+#include <moveit/robot_state/conversions.h>
+#include <moveit/planning_interface/planning_interface.h>
+#include <moveit_msgs/MotionPlanRequest.h>
+#include <moveit/planning_scene/planning_scene.h>
+
+#include  "auto_exchange_planner/auto_exchange_context.h"
+#include  "auto_exchange_planner/auto_exchange_planner.h"
+namespace auto_exchange_planner
+{
+AutoExchangeContext::AutoExchangeContext(const std::string& context_name, const std::string& ns,
+                                         const std::string& group_name, const moveit::core::RobotModelConstPtr& model)
+  : planning_interface::PlanningContext(context_name, group_name), robot_model_(model)
+{
+  auto_exchange_planner_ = AutoExchangePlannerPtr(new AutoExchangePlanner(ros::NodeHandle(ns)));
+}
+
+bool AutoExchangeContext::solve(planning_interface::MotionPlanDetailedResponse& res)
+{
+  moveit_msgs::MotionPlanDetailedResponse res_msg;
+  bool auto_exchange_solved = auto_exchange_planner_->solve(planning_scene_, request_, res_msg);
+
+  if (auto_exchange_solved)
+  {
+    res.trajectory_.resize(1);
+    res.trajectory_[0] =
+        robot_trajectory::RobotTrajectoryPtr(new robot_trajectory::RobotTrajectory(robot_model_, getGroupName()));
+
+    moveit::core::RobotState start_state(robot_model_);
+    moveit::core::robotStateMsgToRobotState(res_msg.trajectory_start, start_state);
+
+    res.trajectory_[0]->setRobotTrajectoryMsg(start_state, res_msg.trajectory[0]);
+    res.description_.push_back("plan");
+    res.processing_time_ = res_msg.processing_time;
+    res.error_code_ = res_msg.error_code;
+
+    return true;
+  }
+
+  res.error_code_ = res_msg.error_code;
+  return false;
+};
+
+bool AutoExchangeContext::solve(planning_interface::MotionPlanResponse& res)
+{
+  planning_interface::MotionPlanDetailedResponse res_detailed;
+  bool planning_success = solve(res_detailed);
+
+  res.error_code_ = res_detailed.error_code_;
+
+  if (planning_success)
+  {
+    res.trajectory_ = res_detailed.trajectory_[0];
+    res.planning_time_ = res_detailed.processing_time_[0];
+  }
+
+  return planning_success;
+}
+
+bool AutoExchangeContext::terminate()
+{
+  return true;
+}
+
+void AutoExchangeContext::clear()
+{
+  // This planner has no state, so has nothing to clear
+}
+
+}  // namespace auto_exchange_planner

auto_exchange_planner/src/auto_exchange_manager.cpp

@@ -0,0 +1,85 @@
+//
+// Created by ch on 24-12-6.
+//
+
+#include <moveit/planning_interface/planning_interface.h>
+#include <moveit/planning_interface/planning_response.h>
+#include <moveit/planning_scene/planning_scene.h>
+#include <moveit/collision_detection_fcl/collision_detector_allocator_fcl.h>
+#include <class_loader/class_loader.hpp>
+#include "auto_exchange_planner/auto_exchange_context.h"
+
+namespace auto_exchange_planner
+{
+class AutoExchangeManager : public planning_interface::PlannerManager
+{
+public:
+  AutoExchangeManager() : planning_interface::PlannerManager()
+  {
+  }
+
+  bool initialize(const moveit::core::RobotModelConstPtr& model, const std::string& ns) override
+  {
+    for (const std::string& gpName : model->getJointModelGroupNames())
+    {
+      planning_contexts_[gpName] =
+          AutoExchangeContextPtr(new AutoExchangeContext("auto_exchange_context", ns, gpName, model));
+    }
+    return true;
+  }
+
+  bool canServiceRequest(const moveit_msgs::MotionPlanRequest& req) const override
+  {
+    return req.trajectory_constraints.constraints.empty();
+  }
+
+  std::string getDescription() const override
+  {
+    return "AutoExchange";
+  }
+
+  void getPlanningAlgorithms(std::vector<std::string>& algs) const override
+  {
+    algs.clear();
+    algs.push_back("auto_exchange");
+  }
+
+  planning_interface::PlanningContextPtr getPlanningContext(const planning_scene::PlanningSceneConstPtr& planning_scene,
+                                                            const planning_interface::MotionPlanRequest& req,
+                                                            moveit_msgs::MoveItErrorCodes& error_code) const override
+  {
+    error_code.val = moveit_msgs::MoveItErrorCodes::SUCCESS;
+
+    if (req.group_name.empty())
+    {
+      ROS_ERROR("No group specified to plan for");
+      error_code.val = moveit_msgs::MoveItErrorCodes::INVALID_GROUP_NAME;
+      return planning_interface::PlanningContextPtr();
+    }
+
+    if (!planning_scene)
+    {
+      ROS_ERROR("No planning scene supplied as input");
+      error_code.val = moveit_msgs::MoveItErrorCodes::FAILURE;
+      return planning_interface::PlanningContextPtr();
+    }
+
+    // retrieve and configure existing context
+    const AutoExchangeContextPtr& context = planning_contexts_.at(req.group_name);
+    ROS_INFO_STREAM_NAMED("auto_exchange_manager", "===>>> context is made ");
+
+    context->setPlanningScene(planning_scene);
+    context->setMotionPlanRequest(req);
+
+    error_code.val = moveit_msgs::MoveItErrorCodes::SUCCESS;
+    return context;
+  }
+
+protected:
+  std::map<std::string, AutoExchangeContextPtr> planning_contexts_;
+};
+
+}  // namespace auto_exchange_planner
+
+// register the AutoExchangeManager class as a plugin
+CLASS_LOADER_REGISTER_CLASS(auto_exchange_planner::AutoExchangeManager, planning_interface::PlannerManager);