Fixed move_group_interface

Launchfile changes probably irrelevant but i'm committing every change
just in case

src/ar3_config/launch/dev.launch.py

@@ -54,7 +54,9 @@ 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
@@ -133,7 +135,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": [ "ompl", "move_group"] }
+                { "planning_pipelines": {"pipeline_names":["ompl"], "ompl": {"planning_plugin": "ompl_interface/OMPLPlanner"} } }
             ],
         ),
 

src/ar3_hardware/src/ar3_system.cpp

@@ -14,6 +14,7 @@ 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;
@@ -21,12 +22,15 @@ 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)
   {
-    // DiffBotSystem has exactly two states and one command interface on each joint
+    // We only use one command interface
     if (joint.command_interfaces.size() != 1)
     {
       RCLCPP_FATAL(
@@ -36,6 +40,7 @@ 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(
@@ -45,6 +50,7 @@ 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(
@@ -54,6 +60,7 @@ 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(
@@ -71,6 +78,7 @@ 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++)
   {
@@ -83,6 +91,7 @@ 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++)
   {
@@ -107,6 +116,12 @@ 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_);
@@ -118,6 +133,8 @@ 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);
@@ -125,6 +142,8 @@ 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));
@@ -150,6 +169,10 @@ 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;
 }
@@ -201,6 +224,7 @@ 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 / 40, self._dosomething)
+        self._timer = self.create_timer(1 / 20, self._dosomething)
 
         # Set joint angles independantly
         base_topic = 'move/'

src/move_group_interface/src/move_group_interface_node.cpp

@@ -5,8 +5,9 @@
 #include <thread>
 
 // MoveitCpp
-#include <moveit/moveit_cpp/moveit_cpp.h>
+#include <moveit/common_planning_interface_objects/common_objects.h>
-#include <moveit/moveit_cpp/planning_component.h>
+#include <moveit/move_group_interface/move_group_interface.h>
+#include <moveit/rdf_loader/rdf_loader.h>
 
 #include "geometry_msgs/msg/pose_stamped.hpp"
 
@@ -15,66 +16,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");
 
-        //auto psm = std::make_shared<planning_scene_monitor::PlanningSceneMonitor>("robot_description");
+        move_group_->setPoseTarget(p->pose);
-        //psm->startSceneMonitor();
 
-        // ??????
+        moveit::planning_interface::MoveGroupInterface::Plan plan;
-        moveit_cpp_ptr->getPlanningSceneMonitor()->providePlanningSceneService();
+        bool success = (move_group_->plan(plan) == moveit::planning_interface::MoveItErrorCode::SUCCESS);
 
+        RCLCPP_INFO(this->get_logger(), "Did work? %s", success ? "yes" : "no");
 
-        // planning_scene_monitor::LockedPlanningSceneRW scene{moveit_cpp_ptr->getPlanningSceneMonitor()};
+        move_group_->move();
-    	RCLCPP_INFO(this->get_logger(), "NEW PLANNING");
+    }
 
-        auto planning_components = std::make_shared<moveit_cpp::PlanningComponent>(PLANNING_GROUP, moveit_cpp_ptr);
+    void relativeMoveCallback(geometry_msgs::msg::PoseStamped::SharedPtr p) {
-    	RCLCPP_INFO(this->get_logger(), "GET MODEL");
+    	RCLCPP_INFO(this->get_logger(), "GOT MSG");
-        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");
+        moveit::core::RobotStatePtr robot_state = move_group_->getCurrentState();
-        planning_components->setStartStateToCurrentState();
+        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");
 
-        // Set the goal to the request pose
+        geometry_msgs::msg::Pose target_pose;
-    	RCLCPP_INFO(this->get_logger(), "SET GOAL");
+        target_pose.position.x = 0.24;
-        planning_components->setGoal(*p, "base_link");
+        target_pose.position.y = -0.32;
+        target_pose.position.z = 0.56;
+        move_group_->setPoseTarget(target_pose);
 
-        // Actually plan
+        moveit::planning_interface::MoveGroupInterface::Plan plan;
-    	RCLCPP_INFO(this->get_logger(), "DO PLAN");
+        bool success = (move_group_->plan(plan) == moveit::planning_interface::MoveItErrorCode::SUCCESS);
-        auto plan_solution = planning_components->plan();
 
-        // Plan is valid
+        RCLCPP_INFO(this->get_logger(), "Did work? %s", success ? "yes" : "no");
-        if (plan_solution) {
+
-            // Follow through with the planned path
+        move_group_->move();
-            planning_components->execute();
-        }
     }
 private:
     rclcpp::Subscription<geometry_msgs::msg::PoseStamped>::SharedPtr pose_sub_;
+    rclcpp::Subscription<geometry_msgs::msg::PoseStamped>::SharedPtr relative_pose_sub_;
 
-    //please
+    moveit::planning_interface::MoveGroupInterfacePtr move_group_;
-    //rclcpp::Node::SharedPtr nh_;
+    moveit::core::RobotModelConstPtr robot_model_;
-    // moveit_cpp::MoveItCpp::SharedPtr moveit_cpp_ptr_;
-    // rclcpp::executors::SingleThreadedExecutor::SharedPtr executor_;
-    // std::thread thread_executor_spin_;
 };
 
 int main(int argc, char* argv[]) {
@@ -83,12 +84,9 @@ 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;