src/ar3_config/launch/dev.launch.py

@@ -54,9 +54,7 @@ def generate_launch_description():
                 get_yaml(join(config_root, 'ompl_planning.yaml')),
                 get_yaml(join(config_root, 'trajectory.yaml')),
                 get_yaml(join(config_root, 'planning_scene.yaml')),
-                get_yaml(join(config_root, "ar3_controllers.yaml")),
+                get_yaml(join(config_root, "ar3_controllers.yaml"))],
-                { "default_planning_pipeline": "ompl" },
-                { "planning_pipelines": {"pipeline_names":["ompl"], "ompl": {"planning_plugin": "ompl_interface/OMPLPlanner"} } }]
         ),
 
         # Our joystick node
@@ -135,7 +133,7 @@ def generate_launch_description():
                 get_yaml(join(config_root, 'planning_scene.yaml')),
                 get_yaml(join(config_root, "ar3_controllers.yaml")),
                 { "default_planning_pipeline": "move_group" },
-                { "planning_pipelines": {"pipeline_names":["ompl"], "ompl": {"planning_plugin": "ompl_interface/OMPLPlanner"} } }
+                { "planning_pipelines": [ "ompl", "move_group"] }
             ],
         ),
 

src/ar3_hardware/src/ar3_system.cpp

@@ -14,7 +14,6 @@ namespace ar3_hardware
 hardware_interface::return_type Ar3SystemHardware::configure(
   const hardware_interface::HardwareInfo & info)
 {
-  // Initialize default configuration
   if (configure_default(info) != hardware_interface::return_type::OK)
   {
     return hardware_interface::return_type::ERROR;
@@ -22,15 +21,12 @@ hardware_interface::return_type Ar3SystemHardware::configure(
 
   //hw_start_sec_ = stod(info_.hardware_parameters["example_param_hw_start_duration_sec"]);
   //hw_stop_sec_ = stod(info_.hardware_parameters["example_param_hw_stop_duration_sec"]);
-
-  // Resize position and command vectors to match actual configuration
-  // This makes sure a default value is initialized for each joint and saves
   hw_positions_.resize(info_.joints.size(), std::numeric_limits<double>::quiet_NaN());
   hw_commands_.resize(info_.joints.size(), std::numeric_limits<double>::quiet_NaN());
 
   for (const hardware_interface::ComponentInfo & joint : info_.joints)
   {
-    // We only use one command interface
+    // DiffBotSystem has exactly two states and one command interface on each joint
     if (joint.command_interfaces.size() != 1)
     {
       RCLCPP_FATAL(
@@ -40,7 +36,6 @@ hardware_interface::return_type Ar3SystemHardware::configure(
       return hardware_interface::return_type::ERROR;
     }
 
-    // Make sure the command interface is in position mode
     if (joint.command_interfaces[0].name != hardware_interface::HW_IF_POSITION)
     {
       RCLCPP_FATAL(
@@ -50,7 +45,6 @@ hardware_interface::return_type Ar3SystemHardware::configure(
       return hardware_interface::return_type::ERROR;
     }
 
-    // We only publish to one state interface
     if (joint.state_interfaces.size() != 1)
     {
       RCLCPP_FATAL(
@@ -60,7 +54,6 @@ hardware_interface::return_type Ar3SystemHardware::configure(
       return hardware_interface::return_type::ERROR;
     }
 
-    // Make sure the state interface is in position mode
     if (joint.state_interfaces[0].name != hardware_interface::HW_IF_POSITION)
     {
       RCLCPP_FATAL(
@@ -78,7 +71,6 @@ hardware_interface::return_type Ar3SystemHardware::configure(
 
 std::vector<hardware_interface::StateInterface> Ar3SystemHardware::export_state_interfaces()
 {
-  // Tell the controller about all joints that we publish state info for
   std::vector<hardware_interface::StateInterface> state_interfaces;
   for (auto i = 0u; i < info_.joints.size(); i++)
   {
@@ -91,7 +83,6 @@ std::vector<hardware_interface::StateInterface> Ar3SystemHardware::export_state_
 
 std::vector<hardware_interface::CommandInterface> Ar3SystemHardware::export_command_interfaces()
 {
-  // Tell the controller about all joints that we take commands from
   std::vector<hardware_interface::CommandInterface> command_interfaces;
   for (auto i = 0u; i < info_.joints.size(); i++)
   {
@@ -116,12 +107,6 @@ hardware_interface::return_type Ar3SystemHardware::start()
     }
   }
 
-  // Hack to make sure the node stays running by putting it in its own thread.
-  // this is because we wrote the hardware interface in python earlier and
-  // the driver just listens on some subscribers. This allows a node in this
-  // controller to publish values to that driver
-  // The correct thing to do would be to move all of the driver code into this
-  // controller and remove the node, publishers, and subscribers entierly
   executor_ = std::make_shared<rclcpp::executors::MultiThreadedExecutor>();
   nh_ = std::make_shared<rclcpp::Node>("ar3_hardware");
   executor_->add_node(nh_);
@@ -133,8 +118,6 @@ hardware_interface::return_type Ar3SystemHardware::start()
   };
   thread_executor_spin_ = std::thread(spin);
 
-  // Create joint state publishers
-  // TODO: Don't use Float32, use Pose
   j1_pub_ = nh_->create_publisher<std_msgs::msg::Float32>("/move/j1", 10);
   j2_pub_ = nh_->create_publisher<std_msgs::msg::Float32>("/move/j2", 10);
   j3_pub_ = nh_->create_publisher<std_msgs::msg::Float32>("/move/j3", 10);
@@ -142,8 +125,6 @@ hardware_interface::return_type Ar3SystemHardware::start()
   j5_pub_ = nh_->create_publisher<std_msgs::msg::Float32>("/move/j5", 10);
   j6_pub_ = nh_->create_publisher<std_msgs::msg::Float32>("/move/j6", 10);
 
-  // Create joint command subscribers
-  // TODO: Don't use Float32, use Pose
   s_j1 = nh_->create_subscription<std_msgs::msg::Float32>("/update/j1", 10, std::bind(&Ar3SystemHardware::update_j1_cb, this, std::placeholders::_1));
   s_j2 = nh_->create_subscription<std_msgs::msg::Float32>("/update/j2", 10, std::bind(&Ar3SystemHardware::update_j2_cb, this, std::placeholders::_1));
   s_j3 = nh_->create_subscription<std_msgs::msg::Float32>("/update/j3", 10, std::bind(&Ar3SystemHardware::update_j3_cb, this, std::placeholders::_1));
@@ -169,10 +150,6 @@ hardware_interface::return_type Ar3SystemHardware::stop()
   return hardware_interface::return_type::OK;
 }
 
-// These callbacks are called from the command subscribers, they update
-// the hw_positions_ array with the current joint state that will be
-// sent to the controller.
-// Again, this is wrong and these values should be filled in by the read() function
 void Ar3SystemHardware::update_j1_cb(std_msgs::msg::Float32::SharedPtr msg) {
     hw_positions_[0] = msg->data;
 }
@@ -224,7 +201,6 @@ hardware_interface::return_type ar3_hardware::Ar3SystemHardware::write()
 {
   //RCLCPP_INFO(rclcpp::get_logger("Ar3SystemHardware"), "Writing...");
 
-  // Publish joint commands to the arm driver
   auto message = std_msgs::msg::Float32();
   message.data = hw_commands_[0];
   j1_pub_->publish(message);

src/cobot_platform/cobot_platform/move.py

@@ -23,7 +23,7 @@ class CobotMove(Node):
         self._cur = [0.0] * 6
 
         # create a timer to update all the joint angles at once
-        self._timer = self.create_timer(1 / 20, self._dosomething)
+        self._timer = self.create_timer(1 / 40, self._dosomething)
 
         # Set joint angles independantly
         base_topic = 'move/'

src/move_group_interface/src/move_group_interface_node.cpp

@@ -5,9 +5,8 @@
 #include <thread>
 
 // MoveitCpp
-#include <moveit/common_planning_interface_objects/common_objects.h>
+#include <moveit/moveit_cpp/moveit_cpp.h>
-#include <moveit/move_group_interface/move_group_interface.h>
+#include <moveit/moveit_cpp/planning_component.h>
-#include <moveit/rdf_loader/rdf_loader.h>
 
 #include "geometry_msgs/msg/pose_stamped.hpp"
 
@@ -16,66 +15,66 @@ const std::string PLANNING_GROUP = "ar3_arm";
 class ScanNPlan : public rclcpp::Node {
 public:
     ScanNPlan() : Node("scan_n_plan") {
+        RCLCPP_INFO(this->get_logger(), "SNP START");
+
         // Create /move_pose subscriber
         pose_sub_ = this->create_subscription<geometry_msgs::msg::PoseStamped>(
             "move_pose",
             rclcpp::QoS(1),
             std::bind(&ScanNPlan::moveCallback, this, std::placeholders::_1)
         );
-
+        RCLCPP_INFO(this->get_logger(), "SUB EXIST");
-        relative_pose_sub_ = this->create_subscription<geometry_msgs::msg::PoseStamped>(
-            "move_relative_pose",
-            rclcpp::QoS(1),
-            std::bind(&ScanNPlan::relativeMoveCallback, this, std::placeholders::_1)
-        );
-    }
-
-    void init() {
-        move_group_ = std::make_shared<moveit::planning_interface::MoveGroupInterface>(
-                shared_from_this(), std::string("ar3_arm"));
     }
 
 private:
     void moveCallback(geometry_msgs::msg::PoseStamped::SharedPtr p) {
+	    (void) p;
     	RCLCPP_INFO(this->get_logger(), "GOT MSG");
+        // Get a shared pointer to the MoveItCpp library to use our planning scene and getting states
+        auto moveit_cpp_ptr = std::make_shared<moveit_cpp::MoveItCpp>(shared_from_this());
+    	RCLCPP_INFO(this->get_logger(), "GOT MOVEIT");
 
-        move_group_->setPoseTarget(p->pose);
+        //auto psm = std::make_shared<planning_scene_monitor::PlanningSceneMonitor>("robot_description");
+        //psm->startSceneMonitor();
 
-        moveit::planning_interface::MoveGroupInterface::Plan plan;
+        // ??????
-        bool success = (move_group_->plan(plan) == moveit::planning_interface::MoveItErrorCode::SUCCESS);
+        moveit_cpp_ptr->getPlanningSceneMonitor()->providePlanningSceneService();
 
-        RCLCPP_INFO(this->get_logger(), "Did work? %s", success ? "yes" : "no");
 
-        move_group_->move();
+        // planning_scene_monitor::LockedPlanningSceneRW scene{moveit_cpp_ptr->getPlanningSceneMonitor()};
+    	RCLCPP_INFO(this->get_logger(), "NEW PLANNING");
+
+        auto planning_components = std::make_shared<moveit_cpp::PlanningComponent>(PLANNING_GROUP, moveit_cpp_ptr);
+    	RCLCPP_INFO(this->get_logger(), "GET MODEL");
+        auto robot_model_ptr = moveit_cpp_ptr->getRobotModel();
+    	RCLCPP_INFO(this->get_logger(), "GET START STATE");
+        auto robot_start_state = planning_components->getStartState();
+
+    	RCLCPP_INFO(this->get_logger(), "SETSTART TO CUR");
+        planning_components->setStartStateToCurrentState();
+
+        // Set the goal to the request pose
+    	RCLCPP_INFO(this->get_logger(), "SET GOAL");
+        planning_components->setGoal(*p, "base_link");
+
+        // Actually plan
+    	RCLCPP_INFO(this->get_logger(), "DO PLAN");
+        auto plan_solution = planning_components->plan();
+
+        // Plan is valid
+        if (plan_solution) {
+            // Follow through with the planned path
+            planning_components->execute();
         }
-
-    void relativeMoveCallback(geometry_msgs::msg::PoseStamped::SharedPtr p) {
-    	RCLCPP_INFO(this->get_logger(), "GOT MSG");
-
-        moveit::core::RobotStatePtr robot_state = move_group_->getCurrentState();
-        const Eigen::Isometry3d &end_effector_state = robot_state->getGlobalLinkTransform(move_group_->getEndEffectorLink());
-        RCLCPP_INFO_STREAM(this->get_logger(), "POS: \n" << end_effector_state.translation() << "\n");
-        RCLCPP_INFO_STREAM(this->get_logger(), "ROT: \n" << end_effector_state.rotation() << "\n");
-
-        geometry_msgs::msg::Pose target_pose;
-        target_pose.position.x = 0.24;
-        target_pose.position.y = -0.32;
-        target_pose.position.z = 0.56;
-        move_group_->setPoseTarget(target_pose);
-
-        moveit::planning_interface::MoveGroupInterface::Plan plan;
-        bool success = (move_group_->plan(plan) == moveit::planning_interface::MoveItErrorCode::SUCCESS);
-
-        RCLCPP_INFO(this->get_logger(), "Did work? %s", success ? "yes" : "no");
-
-        move_group_->move();
     }
 private:
     rclcpp::Subscription<geometry_msgs::msg::PoseStamped>::SharedPtr pose_sub_;
-    rclcpp::Subscription<geometry_msgs::msg::PoseStamped>::SharedPtr relative_pose_sub_;
 
-    moveit::planning_interface::MoveGroupInterfacePtr move_group_;
+    //please
-    moveit::core::RobotModelConstPtr robot_model_;
+    //rclcpp::Node::SharedPtr nh_;
+    // moveit_cpp::MoveItCpp::SharedPtr moveit_cpp_ptr_;
+    // rclcpp::executors::SingleThreadedExecutor::SharedPtr executor_;
+    // std::thread thread_executor_spin_;
 };
 
 int main(int argc, char* argv[]) {
@@ -84,9 +83,12 @@ int main(int argc, char* argv[]) {
     auto node = std::make_shared<ScanNPlan>();
     RCLCPP_INFO(node->get_logger(), "MoveGroup node started.");
 
-    node->init();
     rclcpp::spin(node);
 
+    // spin the node
+    // executor_ = std::make_shared<rclcpp::executors::SingleThreadedExecutor>();
+    // executor_->add_node(nh_);
+
     RCLCPP_INFO(node->get_logger(), "Terminating MoveGroup");
     rclcpp::shutdown();
     return 0;